diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..ae19f6c --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,13 @@ +name: Build +on: + - push +jobs: + build: + name: Build + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: actions/setup-java@v1 + with: + java-version: "11.0.2" + - run: ./gradlew build diff --git a/.gitignore b/.gitignore index a1c2a23..983678a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,40 @@ +# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### # Compiled class file *.class @@ -21,3 +58,104 @@ # virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +.classpath +.project +.settings/ +bin/ +imgui.ini + + +# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5200b5c --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,15 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true + } +} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..8612cf2 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2020", + "teamNumber": 1519 +} \ No newline at end of file diff --git a/PathWeaver/Paths/slalom.path b/PathWeaver/Paths/slalom.path new file mode 100644 index 0000000..1589582 --- /dev/null +++ b/PathWeaver/Paths/slalom.path @@ -0,0 +1,14 @@ +X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name +1.261042710938696,-3.894811444030423,2.1092932851819723,0.02981333265274877,true,false, +2.595189347149202,-2.918424799652902,0.23105332805880252,0.4173866571384819,true,false, +3.4895893267316636,-1.956944821601756,0.9208748621429468,0.5341361651409312,false,false, +5.606335945076822,-1.9793048210913178,2.072026619366035,-0.9242133122352103,true,false, +6.865949249655455,-3.3581714562809455,0.3875733244857331,-0.864586646929713,true,false, +7.752895896074729,-3.909718110356797,1.050919976009392,-0.022359999489561577,true,false, +8.774002539431374,-3.0227714639375236,-0.18633332907968025,1.341599969373693,true,false, +7.849789227196162,-2.038931486396815,-1.1627199734571994,0.03726666581593596,true,false, +6.716882586391711,-3.0227714639375236,-0.30558665969067356,-1.5726532974324945,true,false, +5.747949275177379,-3.961891442499107,-1.140359973967639,-0.2981333265274868,true,false, +3.422509328262978,-3.917171443519984,-0.6782533178500332,0.2683199938747385,true,false, +2.0510960262365376,-2.79171813587872,-0.9465733117247719,1.6844532948803024,true,false, +0.5529760604359146,-2.1730914833341846,-0.8422266474401512,-0.03726666581593552,true,false, diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json new file mode 100644 index 0000000..4481449 --- /dev/null +++ b/PathWeaver/pathweaver.json @@ -0,0 +1,9 @@ +{ + "lengthUnit": "Meter", + "exportUnit": "Always Meters", + "maxVelocity": 5.0, + "maxAcceleration": 5.0, + "wheelBase": 5.0, + "gameName": "Slalom Path", + "outputDir": "" +} diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..9eb9d8f --- /dev/null +++ b/build.gradle @@ -0,0 +1,68 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2020.3.2" +} + +sourceCompatibility = JavaVersion.VERSION_11 +targetCompatibility = JavaVersion.VERSION_11 + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project EmbeddedTools. +deploy { + targets { + roboRIO("roborio") { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = frc.getTeamNumber() + } + } + artifacts { + frcJavaArtifact('frcJava') { + targets << "roborio" + // Debug can be overridden by command line, for use with VSCode + debug = frc.getDebugOrDefault(false) + } + // Built in artifact to deploy arbitrary files to the roboRIO. + fileTreeArtifact('frcStaticFileDeploy') { + // The directory below is the local directory to deploy + files = fileTree(dir: 'src/main/deploy') + // Deploy to RoboRIO target, into /home/lvuser/deploy + targets << "roborio" + directory = '/home/lvuser/deploy' + } + } +} + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + implementation wpi.deps.wpilib() + nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop) + + + implementation wpi.deps.vendor.java() + nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) + + testImplementation 'junit:junit:4.12' + + // Enable simulation gui support. Must check the box in vscode to enable support + // upon debugging + simulation wpi.deps.sim.gui(wpi.platforms.desktop, false) +} + +// 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 +// knows where to look for our Robot Class. +jar { + from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..cc4fdc2 Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..d050f17 --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew new file mode 100755 index 0000000..2fe81a7 --- /dev/null +++ b/gradlew @@ -0,0 +1,183 @@ +#!/usr/bin/env sh + +# +# Copyright 2015 the original author or authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >/dev/null +APP_HOME="`pwd -P`" +cd "$SAVED" >/dev/null + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn () { + echo "$*" +} + +die () { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; + NONSTOP* ) + nonstop=true + ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin or MSYS, switch paths to Windows format before running java +if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then + APP_HOME=`cygpath --path --mixed "$APP_HOME"` + CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + JAVACMD=`cygpath --unix "$JAVACMD"` + + # We build the pattern for arguments to be converted via cygpath + ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` + SEP="" + for dir in $ROOTDIRSRAW ; do + ROOTDIRS="$ROOTDIRS$SEP$dir" + SEP="|" + done + OURCYGPATTERN="(^($ROOTDIRS))" + # Add a user-defined pattern to the cygpath arguments + if [ "$GRADLE_CYGPATTERN" != "" ] ; then + OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" + fi + # Now convert the arguments - kludge to limit ourselves to /bin/sh + i=0 + for arg in "$@" ; do + CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` + CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option + + if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition + eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` + else + eval `echo args$i`="\"$arg\"" + fi + i=`expr $i + 1` + done + case $i in + 0) set -- ;; + 1) set -- "$args0" ;; + 2) set -- "$args0" "$args1" ;; + 3) set -- "$args0" "$args1" "$args2" ;; + 4) set -- "$args0" "$args1" "$args2" "$args3" ;; + 5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; + 6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; + 7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; + 8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; + 9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; + esac +fi + +# Escape application args +save () { + for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done + echo " " +} +APP_ARGS=`save "$@"` + +# Collect all arguments for the java command, following the shell quoting and substitution rules +eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" + +exec "$JAVACMD" "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100644 index 0000000..9618d8d --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,100 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem https://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windows variants + +if not "%OS%" == "Windows_NT" goto win9xME_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/settings.gradle b/settings.gradle new file mode 100644 index 0000000..81f96ab --- /dev/null +++ b/settings.gradle @@ -0,0 +1,27 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2020' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + def homeRoot = new File(publicFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } else { + def userFolder = System.getProperty("user.home") + def homeRoot = new File(userFolder, "wpilib") + frcHome = new File(homeRoot, frcYear) + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/src/main/deploy/example.txt b/src/main/deploy/example.txt new file mode 100644 index 0000000..bb82515 --- /dev/null +++ b/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'Filesystem.getDeployDirectory' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java new file mode 100644 index 0000000..4d2951a --- /dev/null +++ b/src/main/java/frc/robot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. Unless you know what + * you are doing, do not modify this file except to change the parameter class to the startRobot + * call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..5238997 --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,147 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 frc.robot; + +import java.util.Date; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * The VM is configured to automatically run this class, and to call the + * functions corresponding to each mode, as described in the TimedRobot + * documentation. If you change the name of this class or the package after + * creating this project, you must also update the build.gradle file in the + * project. + */ +public class Robot extends TimedRobot { + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + public static double autonomousStartTime = -1; + + /** + * This function is run when the robot is first started up and should be used + * for any initialization code. + */ + @Override + @SuppressWarnings("deprecation") + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + + SmartDashboard.putString("Robot Loaded on:", new Date().toLocaleString()); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for + * items like diagnostics that you want ran during disabled, autonomous, + * teleoperated and test. + * + *

+ * This runs after the mode specific periodic functions, but before LiveWindow + * and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled + // commands, running already-scheduled commands, removing finished or + // interrupted commands, + // and running subsystem periodic() methods. This must be called from the + // robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** + * This function is called once each time the robot enters Disabled mode. + */ + @Override + public void disabledInit() { + RobotContainer.init(); + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void disabledPeriodic() { + } + + /** + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + // ensure robot "safety" before anything else when entering new mode + RobotContainer.safetyInit(); + + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + + autonomousStartTime = System.currentTimeMillis(); + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // ensure robot "safety" before anything else when entering new mode + RobotContainer.safetyInit(); + + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + + // for safety reasons, call + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { + } + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } + + public static double autonomousTimeElapsed() { + return (System.currentTimeMillis() - autonomousStartTime) / 1000; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/Constants.java b/src/main/java/org/mayheminc/robot2020/Constants.java new file mode 100644 index 0000000..d4cbcb7 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/Constants.java @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean constants. This class should not be used for any other + * purpose. All constants should be declared globally (i.e. public static). Do + * not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the constants are needed, to reduce verbosity. + */ +public final class Constants { + + public final class Talon { + public static final int DRIVE_LEFT_A = 1; // high current + public static final int DRIVE_LEFT_B = 2; // high current + public static final int DRIVE_RIGHT_A = 3; // high current + public static final int DRIVE_RIGHT_B = 4; // high current + + public static final int SHOOTER_WHEEL_LEFT = 17; // high current + public static final int SHOOTER_WHEEL_RIGHT = 5; // high current + public static final int SHOOTER_FEEDER = 6; + public static final int SHOOTER_TURRET = 7; + public static final int SHOOTER_HOOD = 8; + + public static final int INTAKE_ROLLERS = 9; + public static final int INTAKE_PIVOT = 10; + + public static final int REVOLVER_TURNTABLE = 11; + public static final int CHIMNEY_ROLLER = 12; + + public static final int CLIMBER_WINCH_LEFT = 14; // high current + public static final int CLIMBER_WINCH_RIGHT = 13; // high current + public static final int CLIMBER_WALKER_LEFT = 15; + public static final int CLIMBER_WALKER_RIGHT = 16; + + public static final int CONTROL_PANEL_ROLLER = 18; // WON'T FIT!!! + } + + public final class Solenoid { + public static final int SPARE_0 = 0; + public static final int SPARE_1 = 1; + public static final int SPARE_2 = 2; + public static final int CLIMBER_PISTONS = 3; + + public static final int CAMERA_LIGHTS = 7; + } + + public final class PDP { + public static final int DRIVE_RIGHT_A = 1; + public static final int DRIVE_RIGHT_B = 2; + public static final int DRIVE_LEFT_A = 3; + public static final int DRIVE_LEFT_B = 4; + } + + public final class Drive { + public static final int DRIVEBASE_WIDTH = 25; + + /** + * A correction applied to the radius of arcing turns (inches) + */ + public static final double TURNING_CORRECTION = 1.00; + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/RobotContainer.java b/src/main/java/org/mayheminc/robot2020/RobotContainer.java new file mode 100644 index 0000000..0e87513 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/RobotContainer.java @@ -0,0 +1,320 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020; + +import org.mayheminc.util.*; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import java.util.LinkedList; +import java.util.Map; +import java.util.Map.Entry; +import java.util.function.Supplier; + +import org.mayheminc.robot2020.autonomousroutines.*; +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.*; + +/** + * This class is where the bulk of the robot should be declared. Since + * Command-based is a "declarative" paradigm, very little robot logic should + * actually be handled in the {@link Robot} periodic methods (other than the + * scheduler calls). Instead, the structure of the robot (including subsystems, + * commands, and button mappings) should be declared here. + */ +public class RobotContainer { + public enum GalacticSearchPath { + PATH_A_RED, PATH_A_BLUE, PATH_B_RED, PATH_B_BLUE, UNKNOWN + } + + // The robot's subsystems and commands are defined here... + + public static final Climber climber = new Climber(); + public static final Revolver revolver = new Revolver(); + public static final ShooterWheel shooterWheel = new ShooterWheel(); + public static final Hood hood = new Hood(); + public static final Turret turret = new Turret(); + public static final Feeder feeder = new Feeder(); + public static final Drive drive = new Drive(); + public static final Intake intake = new Intake(); + public static final Autonomous autonomous = new Autonomous(); + public static final Targeting targeting = new Targeting(); + public static final AirCompressor compressor = new AirCompressor(); + public static PidTuner pidtuner; + public static final Chimney chimney = new Chimney(); + + public static final Solenoid cameraLights = new Solenoid(Constants.Solenoid.CAMERA_LIGHTS); + + // Operator Inputs + public static final MayhemDriverStick DRIVER_STICK = new MayhemDriverStick(); + public static final MayhemDriverPad DRIVER_PAD = new MayhemDriverPad(); + public static final MayhemOperatorPad OPERATOR_PAD = new MayhemOperatorPad(); + // private final MayhemOperatorStick OPERATOR_STICK = new MayhemOperatorStick(); + + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ + public RobotContainer() { + // Configure the button bindings + configureButtonBindings(); + configureAutonomousPrograms(); + configureDefaultCommands(); + + // pidtuner = new + // PidTuner(RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SIX, + // RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_SEVEN, + // RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_ELEVEN, + // RobotContainer.DRIVER_STICK.DRIVER_STICK_ENA_BUTTON_TEN, intake); + + cameraLights.set(true); + } + + public static void init() { + shooterWheel.init(); + } + + public static void safetyInit() { + hood.setVBus(0.0); + turret.setVBus(0.0); + climber.setPistons(false); + } + + private void configureDefaultCommands() { + drive.setDefaultCommand(new DriveDefault()); + // intake.setDefaultCommand(new IntakeExtenderVBus()); + revolver.setDefaultCommand(new RevolverDefault()); + + // KBS doesn't think the below is the right way to have the compressor be on "by + // default" because it would require there to always be a command running to + // keep the compressor off. + // However, that is a good way to ensure it doesn't get left off by accident. + // Not quite sure how to handle this; + // would really rather that other commands which need the compressor off (such + // as a high-power command which wants all the battery power available) would + // turn the compressor off when the command starts and off when the command + // ends.) Then again, maybe the "defaultCommand" is a good way to do this + // and I just don't understand the style yet. + // compressor.setDefaultCommand(new AirCompressorDefault()); + } + + private void configureAutonomousPrograms() { + LinkedList autonomousPrograms = new LinkedList(); + + // SelectCommand driveGalacticSearch = new SelectCommand( + // Map.ofEntries(Map.entry(GalacticSearchPath.PATH_A_RED, new PathARed()), + // Map.entry(GalacticSearchPath.PATH_A_BLUE, new PathABlue()), + // Map.entry(GalacticSearchPath.PATH_B_RED, new PathBRed()), + // Map.entry(GalacticSearchPath.PATH_B_BLUE, new PathBBlue()), + // Map.entry(GalacticSearchPath.UNKNOWN, new PrintCommand("-------- no path + // found"))), + // () -> { + // String path = SmartDashboard.getString("GalacticSearchPath", "unknown"); + // GalacticSearchPath pathValue = GalacticSearchPath.UNKNOWN; + + // switch (path) { + // case "path a red": + // pathValue = GalacticSearchPath.PATH_A_RED; + // break; + // case "path b red": + // pathValue = GalacticSearchPath.PATH_B_RED; + // break; + // case "path a blue": + // pathValue = GalacticSearchPath.PATH_A_BLUE; + // break; + // case "path b blue": + // pathValue = GalacticSearchPath.PATH_B_BLUE; + // break; + // } + + // return pathValue; + // }); + + // autonomousPrograms.push(/* 22 */ new DriveTest()); + // autonomousPrograms.push(/* 21 */ driveGalacticSearch); + // autonomousPrograms.push(/* 20 */ driveGalacticSearch); + // autonomousPrograms.push(/* 19 */ new PathBRed()); + // autonomousPrograms.push(/* 18 */ new PathARed()); + // autonomousPrograms.push(/* 17 */ new PathBBlue()); + // autonomousPrograms.push(/* 16 */ new PathABlue()); + // autonomousPrograms.push(/* 15 */ new DriveBouncePath()); + // autonomousPrograms.push(/* 14 */ new DriveBarrelRacing()); + // autonomousPrograms.push(/* 13 */ new DriveSlalom()); + autonomousPrograms.push(new RiverRageForwardShoot3()); + // autonomousPrograms.push(/* 12 */ new StayStill()); + // autonomousPrograms.push(/* 11 */ new StartBWDriveOnlyToRP()); + // autonomousPrograms.push(/* 10 */ new StartBWDriveOnlyToWall()); + // autonomousPrograms.push(/* 09 */ new StartFWDriveOnlyToRP()); + // autonomousPrograms.push(/* 08 */ new StartFWDriveOnlyToWall()); + // autonomousPrograms.push(/* 07 */ new StartBWShoot3ThenToRP()); + // autonomousPrograms.push(/* 06 */ new StartBWShoot3ThenToWall()); + // autonomousPrograms.push(/* 05 */ new StartFWShoot3ThenToRP()); + // autonomousPrograms.push(/* 04 */ new StartFWShoot3ThenToWall()); + // autonomousPrograms.push(/* 03 */ new StartFWRendezvous()); + // autonomousPrograms.push(/* 02 */ new StartBWOppTrench()); + // autonomousPrograms.push(/* 01 */ new StartBWTrench3()); + // autonomousPrograms.push(/* 00 */ new StartBWTrench5()); + + autonomous.setAutonomousPrograms(autonomousPrograms); + + } + + /** + * Use this method to define your button->command mappings. Buttons can be + * created by instantiating a {@link GenericHID} or one of its subclasses + * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then + * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() { + configureDriverStickButtons(); + configureDriverPadButtons(); + configureOperatorStickButtons(); + configureOperatorPadButtons(); + } + + private void configureDriverStickButtons() { + + DRIVER_STICK.DRIVER_STICK_BUTTON_ONE_DISABLED.whenPressed(new SystemZeroIncludingGyro()); + // DRIVER_STICK.DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new + // SystemZeroIncludingGyro()); + // DRIVER_STICK_BUTTON_ONE_ENABLED.whenPressed(new SystemZeroWithoutGyro()); + + // // adjust auto parameters + DRIVER_STICK.DRIVER_STICK_BUTTON_THREE.whenPressed(new SelectAutonomousProgram(autonomous, 1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_TWO.whenPressed(new SelectAutonomousProgram(autonomous, -1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_FOUR.whenPressed(new SelectAutonomousDelay(autonomous, -1)); + DRIVER_STICK.DRIVER_STICK_BUTTON_FIVE.whenPressed(new SelectAutonomousDelay(autonomous, 1)); + + // // NOTE: buttons SIX, SEVEN, TEN, ELEVEN are reserved for PidTuner + // DRIVER_STICK_BUTTON_SEVEN.whenPressed(new TestSound()); + + // // zero elements that require zeroing + // DRIVER_STICK_BUTTON_EIGHT.whenPressed(new DriveZeroGyro()); + // DRIVER_STICK_BUTTON_NINE.whenPressed(new Wait(0)); + } + + private void configureDriverPadButtons() { + + // Debug shooter vbus + // DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new + // ShooterAdjustWheelVBus(0.1)); + // DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new + // ShooterAdjustWheelVBus(-0.1)); + // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterSetWheelVBus(0.0)); + + // Debug Turret + // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new TurretSetAbs(-5500));// + // about -30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new TurretSetAbs(+5500));// + // about +30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whileHeld(new ShooterSetTurretVBus(-0.2));// + // about -30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whileHeld(new + // ShooterSetTurretVBus(+0.2));// about +30 degrees + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new + // HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new + // HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + + DRIVER_PAD.DRIVER_PAD_D_PAD_LEFT.whenPressed(new TurretSetRel(-200.0)); + DRIVER_PAD.DRIVER_PAD_D_PAD_RIGHT.whenPressed(new TurretSetRel(+200.0)); + DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new DriveStraightOnHeading(-0.25, 8)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new TurretSetAbs(+0.0)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new + // ShooterSetHoodAbs(Shooter.HOOD_TARGET_ZONE_POSITION)); + + // Debug Hood + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenPressed(new HoodSetRel(+100)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whenPressed(new HoodSetRel(-100)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whileHeld(new ShooterSetHoodVBus(+1.0)); + // DRIVER_PAD.DRIVER_PAD_D_PAD_DOWN.whileHeld(new ShooterSetHoodVBus(-1.0)); + + // Debug shooter pid velocity + // DRIVER_PAD.DRIVER_PAD_D_PAD_UP.whenHeld(new ParallelCommandGroup( + // new FeederSet(0.8), + // new ChimneySet(0.8) + // )); + // DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whileHeld(new ShooterWheelAdjust(50.0)); + // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whileHeld(new ShooterWheelAdjust(-50.0)); + // DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whileHeld(new HoodSetRel(50.0)); + // DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whileHeld(new HoodSetRel(-50.0)); + // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ShooterWheelSetVBus(0.0)); + + // DRIVER_PAD.DRIVER_PAD_BLUE_BUTTON.whenPressed(new ChimneySet(-0.7)); + // DRIVER_PAD.DRIVER_PAD_GREEN_BUTTON.whenPressed(new FeederSet(0.8)); + // DRIVER_PAD.DRIVER_PAD_YELLOW_BUTTON.whenPressed(new + // ShooterWheelSet(ShooterWheel.IDLE_SPEED)); + // DRIVER_PAD.DRIVER_PAD_RED_BUTTON.whenPressed(new ParallelCommandGroup( + // new ChimneySet(0.0), + // new FeederSet(0.0), + // new ShooterWheelSet(0.0))); + + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenHeld(new ShooterFiringSequence(60.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); + + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenHeld(new ShooterCloseFiringSequence(60.0)); + DRIVER_PAD.DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON.whenReleased(new ShooterCeaseFire()); + + DRIVER_PAD.DRIVER_PAD_BACK_BUTTON.whenPressed(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 240, 0)); + DRIVER_PAD.DRIVER_PAD_START_BUTTON.whenPressed(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 240, 0)); + } + + private void configureOperatorStickButtons() { + } + + private void configureOperatorPadButtons() { + OPERATOR_PAD.OPERATOR_PAD_BUTTON_ONE.whileHeld(new RevolverSetTurntable(0.2)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TWO.whenPressed(new IntakeSetPosition(Intake.PIVOT_DOWN)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_THREE.whileHeld(new RevolverSetTurntable(-0.2)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new TurretSetAbs(0.0)); + // OPERATOR_PAD.OPERATOR_PAD_BUTTON_FOUR.whenPressed(new + // IntakeSetPosition(Intake.PIVOT_UP)); + + // new ShooterSetWheel(1000)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_FIVE.whileHeld(new ChimneySet(1.0)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SIX.whileHeld(new IntakeSetRollersWhileHeld(-1.0)); + + OPERATOR_PAD.OPERATOR_PAD_BUTTON_SEVEN.whenPressed(new ShooterAimToTarget()); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_EIGHT.whileHeld(new IntakeSetRollersWhileHeld(1.0)); + + OPERATOR_PAD.OPERATOR_PAD_BUTTON_NINE.whenPressed(new ClimberSetPistons(false)); + OPERATOR_PAD.OPERATOR_PAD_BUTTON_TEN.whenPressed(new ClimberSetPistons(true)); + + // OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whenPressed(new + // IntakeSetPosition(RobotContainer.intake.PIVOT_UP)); + // OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whenPressed(new + // IntakeSetPosition(RobotContainer.intake.PIVOT_DOWN)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_LEFT.whileHeld(new TurretSetVBus(-0.4)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_RIGHT.whileHeld(new TurretSetVBus(+0.4)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_UP.whileHeld(new HoodAdjust(+1000.0)); + OPERATOR_PAD.OPERATOR_PAD_D_PAD_DOWN.whileHeld(new HoodAdjust(-1000.0)); + + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_UP.whileHeld(new ClimberSetWinchesPower(1.0)); + OPERATOR_PAD.OPERATOR_PAD_RIGHT_Y_AXIS_DOWN.whileHeld(new ClimberSetWinchesPower(-1.0)); + + // OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_UP.whenPressed(new + // RevolverSetTurntable()); + OPERATOR_PAD.OPERATOR_PAD_LEFT_Y_AXIS_DOWN.whileHeld(new ChimneyUnjam()); + } + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + return new RunAutonomous(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java new file mode 100644 index 0000000..a625ff0 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBarrelRacing.java @@ -0,0 +1,49 @@ +// 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.TurnToHeading; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.commands.TurnToHeading.Direction; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveBarrelRacing extends SequentialCommandGroup { + /** Creates a new DriveBarrelRacing. */ + public DriveBarrelRacing() { + // TODO: make work on actual robot + + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new DriveZeroGyro(0.0)); + addCommands(new IntakeSetPosition(Intake.PIVOT_UP)); + + addCommands(new DriveStraightOnHeading(0.6, 80, 0)); + + addCommands(new TurnToHeading(1.5, 0.6, 180, Direction.RIGHT)); + addCommands(new TurnToHeading(1.5, 0.6, -55, Direction.RIGHT)); + + addCommands(new DriveStraightOnHeading(0.6, 94, -2)); + + addCommands(new TurnToHeading(1.5, 0.6, 180, Direction.LEFT)); + addCommands(new TurnToHeading(1.5, 0.6, 80, Direction.LEFT)); + + addCommands(new DriveStraightOnHeading(0.6, 80, 45)); + + addCommands(new TurnToHeading(1.5, 0.6, -160, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.6, 20 * 12, -180)); + + addCommands(new PrintAutonomousTimeElapsed("BarrelRacing")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java new file mode 100644 index 0000000..3700a30 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveBouncePath.java @@ -0,0 +1,49 @@ +// 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.TurnToHeading; + +import org.mayheminc.robot2020.commands.TurnToHeading.Direction; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class DriveBouncePath extends SequentialCommandGroup { + /** Creates a new DriveBouncePath. */ + public DriveBouncePath() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new DriveZeroGyro(0.0)); + addCommands(new IntakeSetPosition(Intake.PIVOT_UP)); + + addCommands(new DriveStraightOnHeading(0.4, 12, 0)); + addCommands(new TurnToHeading(1, 0.4, -90, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.4, 6, -90)); + + // just hit first ball, now back away + addCommands(new DriveStraightOnHeading(-0.4, 6.5 * 12, -116)); + addCommands(new TurnToHeading(1, -0.4, 100, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(-0.4, 6.5 * 12, 90)); + + // just hit second ball, now back away + addCommands(new DriveStraightOnHeading(0.4, 5.1 * 12, 90)); + addCommands(new TurnToHeading(1, 0.4, 0, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.4, 1.5 * 12, 0)); + addCommands(new TurnToHeading(1, 0.4, -80, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.4, 6 * 12, -90)); + + // just hit third ball, now back away + addCommands(new DriveStraightOnHeading(-0.4, 6, -90)); + addCommands(new TurnToHeading(1, -0.4, -160, Direction.LEFT)); + addCommands(new DriveStraightOnHeading(-0.4, 0.5 * 12, -180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java new file mode 100644 index 0000000..fd13f28 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveSlalom.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; + +import edu.wpi.first.wpilibj2.command.*; + +public class DriveSlalom extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public DriveSlalom() { + addCommands(new DriveZeroGyro(0.0)); + + // // drive forward + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 11, 0)); + // // turn + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 295)); + // // drive forward some more + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 11, + // 0)); + // // turn again + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 65)); + // // forward + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 48, 0)); + // // turn again again + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 54, 270)); + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 54, 185)); + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 60, 115)); + + // // lotta driving + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12 * 12, + // 180)); + // // turn again again 2 electic boogalo + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 78, 245)); + // addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 36, 180)); + + addCommands(new TurnToHeading(20, 0.3, -45, TurnToHeading.Direction.LEFT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 15, 295)); + addCommands(new TurnToHeading(20, 0.4, 0, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 66, 0)); + addCommands(new TurnToHeading(20, 0.3, 50, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 20, 50)); + // COOL TURN + addCommands(new TurnToHeading(1, 0.42, -45, TurnToHeading.Direction.LEFT)); + addCommands(new TurnToHeading(1, 0.42, -135, TurnToHeading.Direction.LEFT)); + addCommands(new TurnToHeading(1, 0.42, 100, TurnToHeading.Direction.LEFT)); + // COOL TURN + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 30, 135)); + addCommands(new TurnToHeading(1, 0.3, -180, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 88, 180)); + addCommands(new TurnToHeading(1, 0.3, -135, TurnToHeading.Direction.RIGHT)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 56, -135)); + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 15, -180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraightOnly.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraightOnly.java new file mode 100644 index 0000000..e44ce7d --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveStraightOnly.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; + +import edu.wpi.first.wpilibj2.command.*; + +public class DriveStraightOnly extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public DriveStraightOnly() { + + addCommands(new DriveZeroGyro(0.0)); + + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 50, 0)); + + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 100, 270)); + + // addCommands(new ShooterSetHoodAbs(Shooter.HOOD_INITIATION_LINE_POSITION)); + // addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 100, 0)); + + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java new file mode 100644 index 0000000..10d36b6 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/DriveTest.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; + +import edu.wpi.first.wpilibj2.command.*; + +public class DriveTest extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public DriveTest() { + addCommands(new DriveZeroGyro(0.0)); + + addCommands(new WaitCommand(5)); + + addCommands(new PrintAutonomousTimeElapsed("yay")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java new file mode 100644 index 0000000..57f0143 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathABlue.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.IntakeSetRollers; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class PathABlue extends SequentialCommandGroup { + /** + * Creates a new PathABlue. + * + * Starts pointing straight from the left of the field + */ + public PathABlue() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands(new DriveZeroGyro(0.0)); + + addCommands(new IntakeSetRollers(-1)); + + addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), + new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 10 * 12, 0))); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 9.5 * 12, -85)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 10 * 12, 50)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 3 * 12, 0)); + + addCommands(new IntakeSetRollers(0)); + addCommands(new PrintAutonomousTimeElapsed("Path A Blue")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java new file mode 100644 index 0000000..56dc7ba --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathARed.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.IntakeSetRollers; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Drive; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class PathARed extends SequentialCommandGroup { + /** + * Creates a new PathABlue. + * + * STARTS IN THE CENTER, FACING AHEAD + */ + public PathARed() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands(new DriveZeroGyro(0)); + + addCommands(new IntakeSetRollers(-1)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + + addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 3 * 12, 0)); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 4 * 12, 40)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 9 * 12, -80)); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 13 * 12, 10)); + + addCommands(new IntakeSetRollers(0)); + addCommands(new PrintAutonomousTimeElapsed("Path A Red")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBBlue.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBBlue.java new file mode 100644 index 0000000..ddae266 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBBlue.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.IntakeSetRollers; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class PathBBlue extends SequentialCommandGroup { + /** + * Creates a new PathABlue. + */ + public PathBBlue() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands(new DriveZeroGyro(-8)); + + addCommands(new IntakeSetRollers(-1)); + + addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), + new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 10 * 12, -8))); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 5.5 * 12, -55)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 12 * 12, 55)); + + addCommands(new IntakeSetRollers(0)); + addCommands(new PrintAutonomousTimeElapsed("Path B Blue")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBRed.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBRed.java new file mode 100644 index 0000000..c5f6845 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/PathBRed.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.IntakeSetPosition; +import org.mayheminc.robot2020.commands.IntakeSetRollers; +import org.mayheminc.robot2020.commands.PrintAutonomousTimeElapsed; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class PathBRed extends SequentialCommandGroup { + /** + * Creates a new PathABlue. + */ + public PathBRed() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + + addCommands(new DriveZeroGyro(44)); + + addCommands(new IntakeSetRollers(-1)); + + addCommands(new ParallelCommandGroup(new IntakeSetPosition(Intake.PIVOT_DOWN), + new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 9 * 12, 44))); + + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 8.5 * 12, -60)); + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 11 * 12, 0)); + + addCommands(new IntakeSetRollers(0)); + addCommands(new PrintAutonomousTimeElapsed("Path B Red")); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/RiverRageForwardShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/RiverRageForwardShoot3.java new file mode 100644 index 0000000..c645c74 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/RiverRageForwardShoot3.java @@ -0,0 +1,31 @@ +// 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.DriveStraightOnHeading; +import org.mayheminc.robot2020.commands.DriveZeroGyro; +import org.mayheminc.robot2020.commands.ShooterCloseFiringSequence; +import org.mayheminc.robot2020.commands.ShooterWheelSet; +import org.mayheminc.robot2020.subsystems.ShooterWheel; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class RiverRageForwardShoot3 extends SequentialCommandGroup { + /** Creates a new RiverRageForwardShoot3. */ + public RiverRageForwardShoot3() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands(new DriveZeroGyro(0)); + + addCommands(new DriveStraightOnHeading(0.4, 6 * 14, 0)); + + addCommands(new ShooterWheelSet(ShooterWheel.CLOSE_SHOOTING_SPEED, true)); + + addCommands(new ShooterCloseFiringSequence(15)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java new file mode 100644 index 0000000..7d44a6c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToRP.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWDriveOnlyToRP extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWDriveOnlyToRP() { + + // start backwards + addCommands(new DriveZeroGyro(180.0)); + + // lower the intake + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java new file mode 100644 index 0000000..196046e --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWDriveOnlyToWall.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWDriveOnlyToWall extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWDriveOnlyToWall() { + + // start backwards + addCommands(new DriveZeroGyro(180.0)); + + // lower the intake + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + + // then, drive towards the wall + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 40, 180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java new file mode 100644 index 0000000..a256b99 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWOppTrench.java @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWOppTrench extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWOppTrench() { + + addCommands(new DriveZeroGyro(180.0)); + + // lower the intake and wait for it to be on before turning on rollers or + // driving forwards + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + addCommands(new Wait(2.5)); + addCommands(new IntakeSetRollers(-1.0)); + + // make sure the hood is down + addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); + + // drive to get balls from opponent's trench + addCommands(new DriveStraightOnHeading(0.2, DistanceUnits.INCHES, 108, 180)); + + // now driven to get the balls from opposing trench + addCommands(new Wait(0.8), // wait for last two balls to get into robot + // new IntakeSetRollers(0), // slow down the intake + new DriveStraightOnHeading(-0.4, DistanceUnits.INCHES, 36, 180)); // start backing up + // slowly + + // backup about halfway across the field + addCommands(new DriveStraightOnHeading(-0.4, DistanceUnits.INCHES, 180, 270)); + + // in shooting position, prepare everything for shooting + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION + 3000.0), + new TurretSetAbs((105.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + // turn on the intake gently while shooting to help balls settle + addCommands(new IntakeSetRollers(-0.2)); + + // wait 2/10 of a second to get some camera data after turning turret + addCommands(new Wait(0.2)); + + // use the "one button" firing sequence + addCommands(new ShooterFiringSequence(6.0)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED)); + addCommands(new IntakeSetRollers(0.0)); + + // turn the wheel off now that the shooting is all done + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java new file mode 100644 index 0000000..be0de1a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWShoot3 extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWShoot3() { + + addCommands(new DriveZeroGyro(180.0)); + + // first, lower the intake, start the shooter wheel, and wait until the turret + // is turned towards the target + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new IntakeSetPosition(Intake.PIVOT_DOWN), // intake down + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), // spin up the shooter + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), // set the hood to a good + // starting position + new TurretSetAbs((180.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); // turn + // turret + // around + + addCommands(new ShooterFiringSequence(1.5)); + + // note that the above turns everything off again when it is done. + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToRP.java new file mode 100644 index 0000000..4c10f54 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToRP.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWShoot3ThenToRP extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWShoot3ThenToRP() { + + // start backwards and shoot the first three balls + addCommands(new StartBWShoot3()); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 12, 180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToWall.java new file mode 100644 index 0000000..f4687e4 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWShoot3ThenToWall.java @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWShoot3ThenToWall extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWShoot3ThenToWall() { + + // start backwards and shoot the first three balls + addCommands(new StartBWShoot3()); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, -40, 180)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java new file mode 100644 index 0000000..8adf283 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWTrench extends SequentialCommandGroup { + /** + * Start backwards, right bumper on the centerline of the goal. + */ + public StartBWTrench(double extraDistance) { + // Note: extra distance planned to be 40 inches + + // start backwards and shoot the first three balls + addCommands(new StartBWShoot3()); + + // then, drive to the trench entrance (jog left a little to get there) + addCommands(new DriveStraightOnHeading(0.4, DistanceUnits.INCHES, 35, 125)); + + // pick up balls while heading down the trench. + addCommands(new ParallelCommandGroup( + // intake while driving down the trench + new IntakeSetRollers(-1.0), + // ensure the hood is down + new HoodSetAbsWhileHeld(Hood.STARTING_POSITION), + // drive the path under the control panel to the end + new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 148 + extraDistance, 180))); + + // now driven to the balls at far end of trench + addCommands(new Wait(0.8), // wait for last two balls to get into robot + new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 12, 180)); // start backing up + // slowly + + // after getting all three balls, go back to shooting position + // first, make sure we drive straight out from under the control panel + addCommands(new DriveStraightOnHeading(-0.6, DistanceUnits.INCHES, 16 + extraDistance, 180)); + addCommands(new IntakeSetRollers(-0.2)); // turn off the intake in case it has been stalled for a while + + // drive diagonally over towards the shooting position, while turning on shooter + // wheels, raising the hood, and re-aiming the turret + addCommands( // + new ParallelCommandGroup( // + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), // + new HoodSetAbsWhileHeld(Hood.INITIATION_LINE_POSITION), // + new TurretSetAbs((168.0 * Turret.TICKS_PER_DEGREE)), // + new DriveStraightOnHeading(-0.5, DistanceUnits.INCHES, 96, 160))); // + + // straighten out again to enable turret to aim to the target + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 24, 180)); + + // turn on the intake gently while shooting to help balls settle + addCommands(new IntakeSetRollers(-0.2)); + + // start the shooting sequence + addCommands(new ShooterFiringSequence(6.0)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED)); + addCommands(new IntakeSetRollers(0.0)); + + // put the hood down now that the shooting is all done + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench3.java new file mode 100644 index 0000000..3dbb108 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench3.java @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWTrench3 extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWTrench3() { + addCommands(new StartBWTrench(0.0)); // use extraDistance of 0 inches to stop before control panel + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java new file mode 100644 index 0000000..204da84 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartBWTrench5.java @@ -0,0 +1,19 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartBWTrench5 extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartBWTrench5() { + addCommands(new StartBWTrench(50)); // use extraDistance of 40 inches for control panel + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java new file mode 100644 index 0000000..021b95f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToRP.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWDriveOnlyToRP extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWDriveOnlyToRP() { + + // start forwards + addCommands(new DriveZeroGyro(0.0)); + + // lower the intake + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + + // then, drive to the RP + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java new file mode 100644 index 0000000..7a4d19a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWDriveOnlyToWall.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWDriveOnlyToWall extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWDriveOnlyToWall() { + + // start forwards + addCommands(new DriveZeroGyro(0.0)); + + // lower the intake + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + + // then, drive to the RP + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 40, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java new file mode 100644 index 0000000..67efa87 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWRendezvous.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWRendezvous extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWRendezvous() { + + // start out facing in the normal direction + addCommands(new DriveZeroGyro(0.0)); + + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + + // shoot the 3 balls we started with + // first, lower the intake, start the shooter wheel, and wait until the turret + // is turned towards the target + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new IntakeSetPosition(Intake.PIVOT_DOWN), new ShooterWheelSet(ShooterWheel.IDLE_SPEED), + new HoodSetAbs(Hood.INITIATION_LINE_POSITION), + new TurretSetAbs((10.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + addCommands(new ShooterFiringSequence(1.5)); + + // drive a little bit out to get away from other robots. + addCommands(new DriveStraightOnHeading(-0.2, DistanceUnits.INCHES, 36, 30)); + + // now that we are clear of other robots, lower the intake while backing up + // further + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); + + // raise the hood a little to shoot from this increased distance + addCommands(new HoodSetAbsWhileHeld((Hood.INITIATION_LINE_POSITION + Hood.TRENCH_MID_POSITION) / 2.0)); + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 124, 40)); + + // turn on the intake to pick up balls + addCommands(new IntakeSetRollers(-1.0)); + addCommands(new DriveStraightOnHeading(0.15, DistanceUnits.INCHES, 72, 65)); + + // back up to get free of the boundary post, then turn and drive towards goal + addCommands(new DriveStraightOnHeading(-0.1, DistanceUnits.INCHES, 16, 20)); + addCommands(new DriveStraightOnHeading(+0.2, DistanceUnits.INCHES, 48, 20)); + + // now driven to get the balls, stay here and shoot them + addCommands(new Wait(0.8), // wait for the balls to get into robot + new IntakeSetRollers(0)); // turn off the intake + + // in shooting position, prepare everything for shooting + addCommands(new ParallelCommandGroup( // run the following commands in parallel: + new ShooterWheelSet(ShooterWheel.TRENCH_FRONT_SPEED), + new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + // turn on the intake gently while shooting to help balls settle + addCommands(new IntakeSetRollers(-0.2)); + addCommands(new ShooterFiringSequence(6.0)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ShooterWheelSet(ShooterWheel.IDLE_SPEED)); + addCommands(new IntakeSetRollers(0.0)); + + // turn the wheel off now that the shooting is all done + addCommands(new HoodSetAbsWhileHeld(Hood.TARGET_ZONE_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java new file mode 100644 index 0000000..6ca58b8 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; +import org.mayheminc.robot2020.subsystems.ShooterWheel; +import org.mayheminc.robot2020.subsystems.Turret; + +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWShoot3 extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWShoot3() { + + addCommands(new DriveZeroGyro(0.0)); + + // first, lower the intake, start the shooter wheel, and wait until the turret + // is turned towards the target + // addCommands(new ParallelCommandGroup( // run the following commands in + // parallel: + // new IntakeSetPosition(Intake.PIVOT_DOWN), + // // new ShooterWheelSet(ShooterWheel.INITIATION_LINE_SPEED, true), + // // new HoodSetAbs(Hood.INITIATION_LINE_POSITION), + // new TurretSetAbs((0.0 * Turret.TICKS_PER_DEGREE), Turret.WAIT_FOR_DONE))); + + addCommands(new ShooterFiringSequence(4.0)); + + // turn the shooter wheel and intake off now that the shooting is all done + addCommands(new ParallelCommandGroup( // below commands in parallel + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), // + new IntakeSetRollers(0.0), // turn off the rollers + new HoodSetAbs(Hood.STARTING_POSITION))); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java new file mode 100644 index 0000000..d81358b --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToRP.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWShoot3ThenToRP extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWShoot3ThenToRP() { + + // start backwards and shoot the first three balls + // addCommands(new StartFWShoot3()); + addCommands(new Wait(2.0)); + + addCommands(new ShooterFiringSequence(4.0)); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(-0.3, DistanceUnits.INCHES, 12, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToWall.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToWall.java new file mode 100644 index 0000000..8b3d637 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StartFWShoot3ThenToWall.java @@ -0,0 +1,26 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.autonomousroutines; + +import org.mayheminc.robot2020.commands.*; +import org.mayheminc.robot2020.commands.DriveStraightOnHeading.DistanceUnits; +import edu.wpi.first.wpilibj2.command.*; + +public class StartFWShoot3ThenToWall extends SequentialCommandGroup { + /** + * Add your docs here. + */ + public StartFWShoot3ThenToWall() { + + // start backwards and shoot the first three balls + addCommands(new StartFWShoot3()); + + // then, drive to the rendezvous point + addCommands(new DriveStraightOnHeading(0.3, DistanceUnits.INCHES, 40, 0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java new file mode 100644 index 0000000..f3d5a35 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/StayStill.java @@ -0,0 +1,24 @@ +/* + * To change this license header, choose License Headers in Project Properties. + * To change this template file, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.robot2020.autonomousroutines; + +import edu.wpi.first.wpilibj2.command.*; +import org.mayheminc.robot2020.commands.DriveZeroGyro; + +/** + * + * @author Team1519 + */ +public class StayStill extends SequentialCommandGroup { + + public StayStill() { + + // Perform needed initialization + addCommands(new DriveZeroGyro(0.0)); + + // ALL DONE! + } +} diff --git a/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java new file mode 100644 index 0000000..18d6afe --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/autonomousroutines/TestTurret.java @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.autonomousroutines; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class TestTurret extends SequentialCommandGroup { + /** + * Creates a new TestTurret. + */ + public TestTurret() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // addRequirements(RobotContainer.shooter); + + // addCommands(new ShooterSetTurretAbs(-10)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(10)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(-20)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(20)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(-45)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + // addCommands(new ShooterSetTurretAbs(45)); + // addCommands(new Wait(3)); + // addCommands(new ShooterSetTurretAbs(0)); + // addCommands(new Wait(3)); + + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java new file mode 100644 index 0000000..816e6d4 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorDefault.java @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class AirCompressorDefault extends CommandBase { + boolean value; + + /** + * Creates a new AirCompressorDefault. + */ + public AirCompressorDefault() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.compressor); + } + + public AirCompressorDefault(boolean b) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.compressor); + value = b; + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // robot.is + RobotContainer.compressor.setCompressor(value); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/AirCompressorPause.java b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorPause.java new file mode 100644 index 0000000..0cca5be --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/AirCompressorPause.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class AirCompressorPause extends InstantCommand { + /** + * Creates a new AirCompressorSet. + */ + public AirCompressorPause() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.compressor); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.compressor.setCompressor(false); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ChimneySet.java b/src/main/java/org/mayheminc/robot2020/commands/ChimneySet.java new file mode 100644 index 0000000..36f2e61 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ChimneySet.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ChimneySet extends CommandBase { + double m_speed; + + /** + * Creates a new ChimneySet. + */ + public ChimneySet(double d) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.chimney); + m_speed = d; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.chimney.setChimneySpeed(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.chimney.setChimneySpeed(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/commands/ChimneyUnjam.java b/src/main/java/org/mayheminc/robot2020/commands/ChimneyUnjam.java new file mode 100644 index 0000000..e14e663 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ChimneyUnjam.java @@ -0,0 +1,18 @@ +// 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.*; + +public class ChimneyUnjam extends SequentialCommandGroup { + /** Creates a new ChimneyUnjam. */ + public ChimneyUnjam() { + super(new ParallelCommandGroup(new RevolverSetTurntable(-0.5), new ChimneySet(-0.5))); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ClimberSetPistons.java b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetPistons.java new file mode 100644 index 0000000..e8e07c1 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetPistons.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ClimberSetPistons extends InstantCommand { + boolean m_piston; + + public ClimberSetPistons(boolean b) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.climber); + m_piston = b; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.climber.setPistons(m_piston); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java new file mode 100644 index 0000000..d8536b5 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ClimberSetWinchesPower.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ClimberSetWinchesPower extends CommandBase { + double power; + + /** + * Creates a new ClimberSetWinchesPower. + */ + public ClimberSetWinchesPower(double d) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.climber); + power = d; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + RobotContainer.climber.setWinchLeftSpeed(power); + RobotContainer.climber.setWinchRightSpeed(power); + } + + @Override + public void end(boolean interrupted) { + + RobotContainer.climber.setWinchLeftSpeed(0.0); + RobotContainer.climber.setWinchRightSpeed(0.0); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ClimberZero.java b/src/main/java/org/mayheminc/robot2020/commands/ClimberZero.java new file mode 100644 index 0000000..42138d1 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ClimberZero.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ClimberZero extends InstantCommand { + public ClimberZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.climber); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.climber.zero(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java b/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java new file mode 100644 index 0000000..4bffe97 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveDefault.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class DriveDefault extends CommandBase { + /** + * Creates a new DriveDefault. + */ + public DriveDefault() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.drive); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double throttle = RobotContainer.DRIVER_PAD.driveThrottle(); + double steeringX = RobotContainer.DRIVER_PAD.steeringX(); + boolean quickTurn = RobotContainer.DRIVER_PAD.quickTurn(); + RobotContainer.drive.speedRacerDrive(throttle, steeringX, quickTurn); + // RobotContainer.drive.speedRacerDrive(0, 0, false); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveStraight.java b/src/main/java/org/mayheminc/robot2020/commands/DriveStraight.java new file mode 100644 index 0000000..4a9c784 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveStraight.java @@ -0,0 +1,52 @@ +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +// import org.mayheminc.robot2019.Robot; +// import org.mayheminc.robot2019.subsystems.Drive; + +// import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class DriveStraight extends CommandBase { + + double m_targetPower; + + /** + * + * @param arg_targetPower +/- motor power [-1.0, +1.0] + * @param arg_distance Distance in encoder counts + */ + public DriveStraight(double arg_targetSpeed) { + addRequirements(RobotContainer.drive); + + m_targetPower = arg_targetSpeed; + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + public void execute() { + RobotContainer.drive.speedRacerDrive(m_targetPower, 0, false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return (false); + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + RobotContainer.drive.stop(); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java new file mode 100644 index 0000000..b188464 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveStraightOnHeading.java @@ -0,0 +1,80 @@ +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import org.mayheminc.robot2020.subsystems.Drive; + +// import org.mayheminc.robot2019.Robot; +// import org.mayheminc.robot2019.subsystems.Drive; + +// import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class DriveStraightOnHeading extends CommandBase { + + double m_targetPower; + double m_desiredDisplacement; + double m_desiredHeading; + + public enum DistanceUnits { + ENCODER_TICKS, INCHES + }; + + public DriveStraightOnHeading(double arg_targetSpeed, double arg_distance) { + this(arg_targetSpeed, DistanceUnits.INCHES, arg_distance, RobotContainer.drive.getHeading()); + } + + public DriveStraightOnHeading(double arg_targetSpeed, double arg_distance, double heading) { + this(arg_targetSpeed, DistanceUnits.INCHES, arg_distance, heading); + } + + /** + * + * @param arg_targetPower +/- motor power [-1.0, +1.0] + * @param arg_distance Distance in encoder counts + */ + public DriveStraightOnHeading(double arg_targetSpeed, DistanceUnits units, double arg_distance, double heading) { + addRequirements(RobotContainer.drive); + + if (units == DistanceUnits.INCHES) { + arg_distance = arg_distance / Drive.DISTANCE_PER_PULSE_IN_INCHES; + } + m_targetPower = arg_targetSpeed; + m_desiredDisplacement = Math.abs(arg_distance); + m_desiredHeading = heading; + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + RobotContainer.drive.saveInitialWheelDistance(); + RobotContainer.drive.setDesiredHeading(m_desiredHeading); + // System.out.println("Starting Routine: Drive Straight On Heading"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + public void execute() { + RobotContainer.drive.speedRacerDrive(m_targetPower, 0, false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + int displacement = (int) RobotContainer.drive.getWheelDistance(); + + displacement = Math.abs(displacement); + // System.out.println("displacement" + displacement); + + return (displacement >= m_desiredDisplacement); + } + + // Called once after isFinished returns true + @Override + public void end(boolean interrupted) { + RobotContainer.drive.stop(); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java new file mode 100644 index 0000000..ff8900c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/DriveZeroGyro.java @@ -0,0 +1,34 @@ +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +// import org.mayheminc.robot2019.Robot; + +// import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class DriveZeroGyro extends RobotDisabledCommand { + + private double m_headingOffset = 0.0; + + public DriveZeroGyro(double headingOffset) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + addRequirements(RobotContainer.drive); + + m_headingOffset = headingOffset; + } + + // Called just before this Command runs the first time + public void initialize() { + RobotContainer.drive.zeroHeadingGyro(m_headingOffset); + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return true; + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/FeederSet.java b/src/main/java/org/mayheminc/robot2020/commands/FeederSet.java new file mode 100644 index 0000000..401b1c5 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/FeederSet.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class FeederSet extends CommandBase { + double m_speed; + + /** + * Creates a new ShooterSetFeeder. + */ + public FeederSet(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.feeder); + + m_speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.feeder.setSpeed(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + RobotContainer.feeder.setSpeed(0.0); + + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodAdjust.java b/src/main/java/org/mayheminc/robot2020/commands/HoodAdjust.java new file mode 100644 index 0000000..e84b602 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodAdjust.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class HoodAdjust extends CommandBase { + + double m_adjust; + + /** + * Creates a new ShooterAdjustHood. + */ + public HoodAdjust(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.setPosition(RobotContainer.hood.getPosition() + m_adjust); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java new file mode 100644 index 0000000..169b9e7 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbs.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class HoodSetAbs extends CommandBase { + double m_set; + + /** + * Creates a new HoodSetAbs. + */ + public HoodSetAbs(double set) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + + m_set = set; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.setPosition(m_set); + } + + @Override + public boolean isFinished() { + return (RobotContainer.hood.isAtPosition()); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java new file mode 100644 index 0000000..505cf0a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetAbsWhileHeld.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class HoodSetAbsWhileHeld extends CommandBase { + double m_set; + + /** + * Creates a new HoodSetAbs. + */ + public HoodSetAbsWhileHeld(double set) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + + m_set = set; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.setPosition(m_set); + } + + @Override + public boolean isFinished() { + return true; + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetRel.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetRel.java new file mode 100644 index 0000000..404e57a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetRel.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class HoodSetRel extends InstantCommand { + double m_adjust; + + /** + * Creates a new ShooterSetHood. + */ + public HoodSetRel(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double pos = RobotContainer.hood.getPosition(); + RobotContainer.hood.setPosition(pos + m_adjust); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodSetVBus.java b/src/main/java/org/mayheminc/robot2020/commands/HoodSetVBus.java new file mode 100644 index 0000000..4f12d42 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodSetVBus.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class HoodSetVBus extends CommandBase { + double m_power; + + /** + * Creates a new ShooterSetHoodVBus. + */ + public HoodSetVBus(double power) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + m_power = power; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.setVBus(m_power); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.hood.setVBus(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/HoodZero.java b/src/main/java/org/mayheminc/robot2020/commands/HoodZero.java new file mode 100644 index 0000000..d5548ad --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/HoodZero.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class HoodZero extends InstantCommand { + public HoodZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.hood.zero(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java new file mode 100644 index 0000000..a471f5a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeExtenderVBus.java @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeExtenderVBus extends CommandBase { + /** + * Creates a new IntakeExtenderVBus. + */ + public IntakeExtenderVBus() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double power = RobotContainer.OPERATOR_PAD.getLeftYAxis(); + double sign = (power >= 0) ? -1.0 : 1.0;// invert the sign + power = sign * power * power; + RobotContainer.intake.setPivotVBus(power); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java new file mode 100644 index 0000000..16f8847 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetPosition.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeSetPosition extends CommandBase { + double m_position; + boolean m_waitForDone; + + /** + * Creates a new IntakeSetPosition, with "wait" set to false + */ + public IntakeSetPosition(double position) { + this (position, false); + } + + /** + * Creates a new IntakeSetPosition + */ + public IntakeSetPosition(double position, boolean wait) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + + m_position = position; + m_waitForDone = wait; + } + + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.setPivot(m_position); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_waitForDone) { + return RobotContainer.intake.isPivotAtPosition(); + } else { + return true; + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java new file mode 100644 index 0000000..c1d8a7a --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollers.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeSetRollers extends CommandBase { + double m_speed; + + /** + * Creates a new IntakeSetRollers. + */ + public IntakeSetRollers(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + m_speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.setRollers(m_speed); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollersWhileHeld.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollersWhileHeld.java new file mode 100644 index 0000000..0ad8098 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeSetRollersWhileHeld.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class IntakeSetRollersWhileHeld extends CommandBase { + double m_speed; + + /** + * Creates a new IntakeSetRollers. + */ + public IntakeSetRollersWhileHeld(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + m_speed = speed; + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.setRollers(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.intake.setRollers(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/IntakeZero.java b/src/main/java/org/mayheminc/robot2020/commands/IntakeZero.java new file mode 100644 index 0000000..78ea1c3 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/IntakeZero.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class IntakeZero extends InstantCommand { + /** + * Creates a new IntakeZero. + */ + public IntakeZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.intake.zero(); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java new file mode 100644 index 0000000..55e94f8 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeElapsed.java @@ -0,0 +1,30 @@ +package org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj.DriverStation; + +import frc.robot.Robot; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class PrintAutonomousTimeElapsed extends CommandBase { + String Message = ""; + + public PrintAutonomousTimeElapsed(String msg) { + this.Message = msg; + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + DriverStation.reportError(Message + " At: " + Robot.autonomousTimeElapsed() + "\n", false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java new file mode 100644 index 0000000..f65cd39 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/PrintAutonomousTimeRemaining.java @@ -0,0 +1,30 @@ +package org.mayheminc.robot2020.commands; + +// import org.mayheminc.robot2020.Robot; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class PrintAutonomousTimeRemaining extends CommandBase { + String Message = ""; + + public PrintAutonomousTimeRemaining(String msg) { + this.Message = msg; + } + + // Called just before this Command runs the first time + @Override + public void initialize() { + + // DriverStation.reportError(Message + " At: " + Robot.autonomousTimeRemaining() + // + "\n", false); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/RevolverDefault.java b/src/main/java/org/mayheminc/robot2020/commands/RevolverDefault.java new file mode 100644 index 0000000..495169c --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/RevolverDefault.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class RevolverDefault extends CommandBase { + /** + * Creates a new RevolverDefault. + */ + public RevolverDefault() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.revolver); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + RobotContainer.revolver.setRevolverSpeed(-RobotContainer.OPERATOR_PAD.getLeftYAxis()); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/RevolverSetTurntable.java b/src/main/java/org/mayheminc/robot2020/commands/RevolverSetTurntable.java new file mode 100644 index 0000000..2dab623 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/RevolverSetTurntable.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class RevolverSetTurntable extends CommandBase { + double m_speed; + + /** + * Creates a new RevolverSetTurntable. + */ + public RevolverSetTurntable(double d) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.revolver); + m_speed = d; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.revolver.setRevolverSpeed(m_speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.revolver.setRevolverSpeed(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/RobotDisabledCommand.java b/src/main/java/org/mayheminc/robot2020/commands/RobotDisabledCommand.java new file mode 100644 index 0000000..57885d0 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/RobotDisabledCommand.java @@ -0,0 +1,23 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class RobotDisabledCommand extends CommandBase { + /** + * Creates a new RobotDisabledCommand. + */ + public RobotDisabledCommand() { + // Use addRequirements() here to declare subsystem dependencies. + } + + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java b/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java new file mode 100644 index 0000000..c2a7826 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/RunAutonomous.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.CommandScheduler; + +/** + * Delay the set amount of time then schedule the selected command to run. + */ +public class RunAutonomous extends CommandBase { + private long startTime; + Command command; + boolean commandIsRunning; + + /** + * Creates a new RunAutonomous. + */ + public RunAutonomous() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called just before this Command runs the first time + public void initialize() { + startTime = System.currentTimeMillis() + RobotContainer.autonomous.getDelay() * 1000; + command = RobotContainer.autonomous.getSelectedProgram(); + } + + // Called repeatedly when this Command is scheduled to run + public void execute() { + if (System.currentTimeMillis() >= startTime) { + CommandScheduler.getInstance().schedule(command); + commandIsRunning = true; + } + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return commandIsRunning; + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousDelay.java b/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousDelay.java new file mode 100644 index 0000000..db6127f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousDelay.java @@ -0,0 +1,39 @@ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Autonomous; + +/** + * + * @author Team1519 + */ +public class SelectAutonomousDelay extends RobotDisabledCommand { + + private int delta; + private Autonomous auto; + + public SelectAutonomousDelay(Autonomous auto, int delta) { + addRequirements(auto); + this.auto = auto; + this.delta = delta; + } + + // Called just before this Command runs the first time + public void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + public void execute() { + auto.adjustDelay(delta); + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + public void end(boolean interrupted) { + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousProgram.java b/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousProgram.java new file mode 100644 index 0000000..f7d4201 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/SelectAutonomousProgram.java @@ -0,0 +1,30 @@ + +package org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Autonomous; + +/** + * + * @author Team1519 + */ +public class SelectAutonomousProgram extends RobotDisabledCommand { + + private int m_delta; + private Autonomous auto; + + public SelectAutonomousProgram(Autonomous auto, int delta) { + addRequirements(auto); + this.auto = auto; + m_delta = delta; + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + public void end(boolean interrupted) { + this.auto.adjustProgramNumber(m_delta); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java new file mode 100644 index 0000000..0069b50 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterAimToTarget.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterAimToTarget extends CommandBase { + /** + * Creates a new TurretAimToTarget. + */ + public ShooterAimToTarget() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + addRequirements(RobotContainer.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // aim the hood and turret + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // aim the hood and turret + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (!interrupted) { + // RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getDesiredWheelSpeed()); + + // want to also schedule a command here that does the shooting! + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (RobotContainer.hood.isAtPosition() && RobotContainer.turret.isAtDesiredPosition()); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java new file mode 100644 index 0000000..f4a93f4 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCeaseFire.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; +import org.mayheminc.robot2020.subsystems.ShooterWheel; + +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterCeaseFire extends SequentialCommandGroup { + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterCeaseFire() { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // turn off the subsystems that were used in shooting + // (feed roller, chimney, revolver, and shooter wheels) + + addCommands( // + new ParallelRaceGroup( // + new FeederSet(0.0), // + new ChimneySet(0.0), // + new RevolverSetTurntable(0.0), // + new ShooterWheelSet(ShooterWheel.IDLE_SPEED), // + new Wait(0.1) // + )); + + // Lower the hood and intake now that we're done shooting + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + addCommands(new IntakeSetPosition(Intake.PIVOT_DOWN)); // ensure intake is lowered,); + addCommands(new IntakeSetRollers(0.0)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java new file mode 100644 index 0000000..5f5b9a9 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterCloseFiringSequence.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Hood; +import org.mayheminc.robot2020.subsystems.Intake; +import org.mayheminc.robot2020.subsystems.ShooterWheel; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterCloseFiringSequence extends SequentialCommandGroup { + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterCloseFiringSequence(double waitDuration) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // Prepare for shooting. + addCommands(new IntakeSetPosition(Intake.PIVOT_SHOOTING)); // move intake to "shooting position" + + addCommands( // + // new ParallelCommandGroup( // prepare for shooting, + new AirCompressorPause() // turn off compressor while actively shooting, + // , new ShooterAimToTarget() // and aim at the target (azimuth and elevation). + // ) + ); + + // no aiming when up close; just turn on the shooter wheels and raise the hood + addCommands(new ShooterWheelSet(ShooterWheel.CLOSE_SHOOTING_SPEED, true), // shooter wheel manual speed + new HoodSetAbs(Hood.CLOSE_SHOOTING_POSITION)); + + // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the + // revolver turntable, shoot for specified duration. + // TODO: should really shoot until no balls detected any more + addCommands(new ParallelRaceGroup( // + new FeederSet(1.0), // + new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), // + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), new Wait(waitDuration))); + + // turn off the feeder, chimney, and revolver, ending after 0.1 seconds + addCommands(new ShooterCeaseFire()); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java new file mode 100644 index 0000000..1196e59 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFireWhenReady.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +// import org.mayheminc.robot2020.subsystems.Targeting; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterFireWhenReady extends CommandBase { + /** + * Creates a new ShooterFireWhenReady. + */ + public ShooterFireWhenReady() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + addRequirements(RobotContainer.feeder); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // boolean wheelsGood = + // Math.abs(Targeting.convertRangeToWheelSpeed(RobotContainer.targeting.getRangeToTarget()) + // - RobotContainer.shooter.getShooterWheelSpeed()) < 100; + + // RobotContainer.shooter.setFeederSpeed((wheelsGood) ? 0.5 : 0.0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.feeder.setSpeed(0); + RobotContainer.shooterWheel.setSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java new file mode 100644 index 0000000..59d61f0 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterFiringSequence.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Intake; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterFiringSequence extends SequentialCommandGroup { + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterFiringSequence(double waitDuration) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // Prepare for shooting. + addCommands(new IntakeSetPosition(Intake.PIVOT_SHOOTING)); // move intake to "shooting position" + addCommands(new ParallelCommandGroup( // prepare for shooting, + new AirCompressorPause(), // turn off compressor while actively shooting, + new ShooterAimToTarget())); // and aim at the target (azimuth and elevation). + + // prior command established aim; turn on the shooter wheels and maintain turret + addCommands( // + new ParallelRaceGroup( // + new ShooterWheelSetToTarget(true), // set the shooter wheel + new TurretAimToTargetContinuously() // set the aim hood and turret to target + )); + + // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the + // revolver turntable, shoot for specified duration. + // TODO: should really shoot until no balls detected any more + addCommands(new ParallelRaceGroup( // + new ShooterWheelSetToTargetContinuously(), // spin up the shooter wheel + new TurretAimToTargetContinuously(), // continue aiming while shooting + new FeederSet(1.0), // turn on the shooter feeder + new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), // wait a bit and then set the chimney + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(1.0)), // + new Wait(waitDuration))); // wait a little longer and then turn on the revolver + + // turn off the feeder, chimney, and revolver, ending after 0.1 seconds + addCommands(new ShooterCeaseFire()); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java new file mode 100644 index 0000000..745db5f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterPermissionToFire.java @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterPermissionToFire extends CommandBase { + + /** + * Creates a new ShooterPositionToFire This command is intended to shoot while + * the driver holds the "Permission To Fire" button + */ + public ShooterPermissionToFire() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.chimney); + addRequirements(RobotContainer.feeder); + addRequirements(RobotContainer.hood); + addRequirements(RobotContainer.revolver); + addRequirements(RobotContainer.shooterWheel); + addRequirements(RobotContainer.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // command the shooterWheel to turn at speed, aimHood, aimTurret + // RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getDesiredWheelSpeed()); + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // command the shooterWheel to turn at speed + // RobotContainer.shooterWheel.setSpeed(RobotContainer.targeting.getWheelSpeedFromY()); + + // aim the hood + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + + // aim the turret + RobotContainer.turret.setPositionAbs(RobotContainer.targeting.getDesiredAzimuth()); + + // if speed is good, turret is aimed, and hood is aimed, start shooting! + // if shooterWheels at speed, turret aimed, and hood aimed, then fire! + // firing actions as follows: + // feed roller after 0.0 seconds since start of firing + // chimney after 0.1 seconds since start of firing + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java new file mode 100644 index 0000000..ea1ac36 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterReadyAimFire.java @@ -0,0 +1,54 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.subsystems.Hood; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class ShooterReadyAimFire extends SequentialCommandGroup { + /** + * Creates a new ShooterReadyAimFire. + */ + public ShooterReadyAimFire(double waitDuration) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + super(); + + // Turn off compressor while actively shooting. + addCommands(new AirCompressorPause()); + + // aim to the target before starting shooting. + // don't continue to next command (to actually start) until both onTarget and up + // to speed + addCommands(new ParallelRaceGroup( + new ParallelCommandGroup(/* new TargetingIsOnTarget(), */ new ShooterWheelSet(3000.0, true)), + new TurretAimToTargetContinuously())); + + // turn on the feeder, wait 0.1, turn on the Chimney, wait 0.1, turn on the + // revolver turntable, shoot for specified duration + // TODO: should really shoot until no balls detected any more + addCommands(new ParallelRaceGroup( // + new FeederSet(1.0), // + new SequentialCommandGroup(new Wait(0.1), new ChimneySet(1.0)), // + new SequentialCommandGroup(new Wait(0.2), new RevolverSetTurntable(0.3)), // + new Wait(waitDuration) // + )); + + // turn off the feeder, chimney, and revolver, ending after 0.1 seconds + addCommands( + new ParallelRaceGroup(new FeederSet(0.0), new ChimneySet(0.0), new RevolverSetTurntable(0.0), new Wait(0.1))); + + // Lower the hood now that we're done shooting + addCommands(new HoodSetAbsWhileHeld(Hood.STARTING_POSITION)); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjust.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjust.java new file mode 100644 index 0000000..5db4626 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjust.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterWheelAdjust extends CommandBase { + + double m_adjust; + + /** + * Creates a new ShooterAdjustWheel. + */ + public ShooterWheelAdjust(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooterWheel.setSpeed(RobotContainer.shooterWheel.getTargetSpeed() + m_adjust); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjustVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjustVBus.java new file mode 100644 index 0000000..d9f3cb6 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelAdjustVBus.java @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterWheelAdjustVBus extends InstantCommand { + double m_adjust; + + /** + * Creates a new ShooterAdjustWheelVBus. + */ + public ShooterWheelAdjustVBus(double adjust) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + m_adjust = adjust; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + double currentSpeed = RobotContainer.shooterWheel.getSpeedVBus(); + RobotContainer.shooterWheel.setSpeedVBus(currentSpeed + m_adjust); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java new file mode 100644 index 0000000..ae0ef83 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSet.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterWheelSet extends CommandBase { + double m_rpm; + boolean m_waitForDone; + + public ShooterWheelSet(double rpm) { + this(rpm, false); + } + + /** + * Creates a new ShooterWheelSet + */ + public ShooterWheelSet(double rpm, boolean wait) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_rpm = rpm; + m_waitForDone = wait; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooterWheel.setSpeed(m_rpm); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if( m_waitForDone) + { + return (Math.abs( m_rpm - RobotContainer.shooterWheel.getSpeed() ) < 100); + } + else + { + return true; + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTarget.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTarget.java new file mode 100644 index 0000000..7e2d2c8 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTarget.java @@ -0,0 +1,49 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterWheelSetToTarget extends CommandBase { + double m_rpm; + boolean m_waitForDone; + + public ShooterWheelSetToTarget() { + this(false); + } + + /** + * Creates a new ShooterWheelSet + */ + public ShooterWheelSetToTarget(boolean wait) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_rpm = 3000; // default setting + m_waitForDone = wait; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_rpm = RobotContainer.targeting.getDesiredWheelSpeed(); + RobotContainer.shooterWheel.setSpeed(m_rpm); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_waitForDone) { + return (Math.abs(m_rpm - RobotContainer.shooterWheel.getSpeed()) < 100); + } else { + return true; + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java new file mode 100644 index 0000000..bc85a41 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetToTargetContinuously.java @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class ShooterWheelSetToTargetContinuously extends CommandBase { + double m_rpm; + // boolean m_waitForDone; + + /** + * Creates a new ShooterWheelSet + */ + public ShooterWheelSetToTargetContinuously() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_rpm = 3000; // default setting + } + + // Called when the command is initially scheduled. + @Override + public void execute() { + m_rpm = RobotContainer.targeting.getDesiredWheelSpeed(); + RobotContainer.shooterWheel.setSpeed(m_rpm); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetVBus.java b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetVBus.java new file mode 100644 index 0000000..15b59ba --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/ShooterWheelSetVBus.java @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ShooterWheelSetVBus extends InstantCommand { + + double m_speed; + /** + * Creates a new ShooterSetWheelVBus. + */ + public ShooterWheelSetVBus(double speed) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.shooterWheel); + + m_speed = speed; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.shooterWheel.setSpeedVBus(m_speed); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java b/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java new file mode 100644 index 0000000..399e276 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/SystemZeroIncludingGyro.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; + +public class SystemZeroIncludingGyro extends ParallelCommandGroup { + /** + * Creates a new SystemZeroIncludingGyro. + */ + public SystemZeroIncludingGyro() { + // Use addRequirements() here to declare subsystem dependencies. + + addCommands(new IntakeZero()); + addCommands(new ClimberZero()); + addCommands(new HoodZero()); + addCommands(new TurretZero()); + addCommands(new DriveZeroGyro(0.0)); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java new file mode 100644 index 0000000..13d6bec --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurnToHeading.java @@ -0,0 +1,111 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurnToHeading extends CommandBase { + double distanceFromPoint; + double speed; + double exitHeading; + double initialHeading; + Direction direction; + int turnPhase; + + double ratio; + + public enum Direction { + LEFT, RIGHT + } + + /** + * Creates a new TurnToHeading. + */ + public TurnToHeading(double distanceFromPoint, double speed, double exitHeading, Direction direction) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.drive); + + this.turnPhase = 0; + this.distanceFromPoint = distanceFromPoint; + this.speed = speed; + this.exitHeading = exitHeading; + this.direction = direction; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.initialHeading = RobotContainer.drive.getHeading(); + this.turnPhase = 0; + + this.ratio = getRatio(); + + if ((speed > 0 && direction == Direction.LEFT) || (speed < 0 && direction == Direction.RIGHT)) { + RobotContainer.drive.tankDrive(speed * ratio, speed); + System.out.println("LEFT: " + speed * ratio); + System.out.println("RIGHT: " + speed); + } else if ((speed > 0 && direction == Direction.RIGHT) || (speed < 0 && direction == Direction.LEFT)) { + RobotContainer.drive.tankDrive(speed, speed * ratio); + System.out.println("LEFT: " + speed); + System.out.println("RIGHT: " + speed * ratio); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + public double getRatio() { + return distanceFromPoint / (distanceFromPoint + Constants.Drive.DRIVEBASE_WIDTH) + / Constants.Drive.TURNING_CORRECTION; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.drive.stop(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (direction == Direction.LEFT) { + if (initialHeading < exitHeading) { + if (turnPhase == 0) { + if (RobotContainer.drive.getHeading() > (initialHeading + 25)) { + turnPhase = 1; + } + return false; + } else { // turnPhase == 1 + return (RobotContainer.drive.getHeading() < exitHeading); + } + } else { + return (RobotContainer.drive.getHeading() < exitHeading + || RobotContainer.drive.getHeading() > (initialHeading + 25)); + } + } else {// direction == right + if (initialHeading > exitHeading) { + if (turnPhase == 0) { + if (RobotContainer.drive.getHeading() < (initialHeading - 25)) { + turnPhase = 1; + } + return false; + } else { // turnPhase == 1 + return (RobotContainer.drive.getHeading() > exitHeading); + } + } else { + return (RobotContainer.drive.getHeading() > exitHeading + || RobotContainer.drive.getHeading() < (initialHeading - 25)); + } + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java new file mode 100644 index 0000000..9ca5346 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretAimToTargetContinuously.java @@ -0,0 +1,51 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretAimToTargetContinuously extends CommandBase { + /** + * Creates a new TurretAimToTarget. + */ + public TurretAimToTargetContinuously() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.hood); + addRequirements(RobotContainer.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + // TODO: may want to only aim at the target if recent valid target data from the + // camera + double pos = RobotContainer.targeting.getDesiredAzimuth(); + // TODO: if the desired azimuth is "beyond the soft stop" should we turn the + // robot? + RobotContainer.turret.setPositionAbs(pos); + RobotContainer.hood.setPosition(RobotContainer.targeting.getDesiredHood()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretIsOnTarget.java b/src/main/java/org/mayheminc/robot2020/commands/TurretIsOnTarget.java new file mode 100644 index 0000000..b230af5 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretIsOnTarget.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +// import org.mayheminc.robot2020.RobotContainer; +// import org.mayheminc.robot2020.subsystems.Targeting; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretIsOnTarget extends CommandBase { + /** + * Creates a new TargetingIsOnTarget. + */ + public TurretIsOnTarget() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + double targetPos = RobotContainer.targeting.getDesiredAzimuth(); + double turretPos = RobotContainer.turret.getPosition(); + + return (Math.abs(targetPos - turretPos) < 50); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java b/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java new file mode 100644 index 0000000..99d14a0 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretSetAbs.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretSetAbs extends CommandBase { + double m_setPoint; + boolean m_waitForDone; + + /** + * Creates a new TurretSetAbs. + */ + public TurretSetAbs(double setPoint) { + this(setPoint, false); + } + + public TurretSetAbs(double setPoint, boolean wait) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.turret); + + m_setPoint = setPoint; + m_waitForDone = wait; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.turret.setPositionAbs(m_setPoint); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_waitForDone) { + return RobotContainer.turret.isAtDesiredPosition(); + } else { + return true; + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretSetRel.java b/src/main/java/org/mayheminc/robot2020/commands/TurretSetRel.java new file mode 100644 index 0000000..6433e88 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretSetRel.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretSetRel extends CommandBase { + double m_setPoint; + + /** + * Creates a new TurretSetRel. + */ + public TurretSetRel(double setPoint) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.turret); + + m_setPoint = setPoint; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.turret.setPositionRel(m_setPoint); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretSetVBus.java b/src/main/java/org/mayheminc/robot2020/commands/TurretSetVBus.java new file mode 100644 index 0000000..17ef5dd --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretSetVBus.java @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class TurretSetVBus extends CommandBase { + double m_power; + + /** + * Creates a new TurretSetVBus. + */ + public TurretSetVBus(double power) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.turret); + m_power = power; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.turret.setVBus(m_power); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + RobotContainer.turret.setVBus(0.0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/TurretZero.java b/src/main/java/org/mayheminc/robot2020/commands/TurretZero.java new file mode 100644 index 0000000..4a9e9fc --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/TurretZero.java @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.commands; + +import org.mayheminc.robot2020.RobotContainer; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class TurretZero extends InstantCommand { + public TurretZero() { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(RobotContainer.turret); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + RobotContainer.turret.zero(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/commands/Wait.java b/src/main/java/org/mayheminc/robot2020/commands/Wait.java new file mode 100644 index 0000000..ae5d78f --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/commands/Wait.java @@ -0,0 +1,34 @@ +package org.mayheminc.robot2020.commands; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; + +/** + * + */ +public class Wait extends CommandBase { + Timer m_Timer = new Timer(); + double m_endTime; + + public Wait() { + this(0); + } + + public Wait(double endTime) { + super(); + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + m_endTime = endTime; + } + + // Called just before this Command runs the first time + public void initialize() { + m_Timer.start(); + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + return m_Timer.hasPeriodPassed(m_endTime); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java new file mode 100644 index 0000000..5dc519e --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/AirCompressor.java @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class AirCompressor extends SubsystemBase { + Compressor compressor = new Compressor(); + Timer m_Timer = new Timer(); + + /** + * Creates a new compressor. + */ + public AirCompressor() { + // turn on the compressor in the constructor + setCompressor(true); + } + + final double COMPRESSOR_PAUSE_TIME = 10.0; + + public void setCompressor(boolean b) { + // b = false; + if (b) { + compressor.start(); + } else { + compressor.stop(); + m_Timer.start(); + } + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + + // if the timer expires, turn on the compressor. + if (m_Timer.hasPeriodPassed(COMPRESSOR_PAUSE_TIME)) { + setCompressor(true); + } + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java new file mode 100644 index 0000000..bae47f1 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Autonomous.java @@ -0,0 +1,91 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.robot2020.subsystems; + +import java.util.LinkedList; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** + * + * @author Team1519 + */ +public class Autonomous extends SubsystemBase { + + private LinkedList autonomousPrograms = new LinkedList(); + + private int programNumber = 0; // 0 = Do nothing + private int delay = 0; + + public Autonomous() { + } + + /** + * Give the autonomous subsystem the list of possible programs to run + * + * @param programs + */ + public void setAutonomousPrograms(LinkedList programs) { + autonomousPrograms = programs; + } + + public Command getSelectedProgram() { + return autonomousPrograms.get(programNumber); + } + + /** + * Returns the delay (0-9). + */ + public int getDelay() { + return delay; + } + + public void adjustProgramNumber(final int delta) { + programNumber += delta; + if (programNumber < 0) { + programNumber = autonomousPrograms.size() - 1; + } else if (programNumber >= autonomousPrograms.size()) { + programNumber = 0; + } + updateSmartDashboard(); + } + + private final int MAX_DELAY = 9; + + @Override + public void periodic() { + updateSmartDashboard(); + } + + public void adjustDelay(final int delta) { + delay += delta; + if (delay < 0) { + delay = 0; + } else if (delay > MAX_DELAY) { + delay = MAX_DELAY; + } + updateSmartDashboard(); + } + + // keep a string buffer in the object so it is not created each iteration. + private StringBuffer sb = new StringBuffer(); + + /** + * Update the smart dashboard with the auto program and delay. + */ + public void updateSmartDashboard() { + sb.setLength(0); + sb.append(programNumber + " " + autonomousPrograms.get(programNumber).getName()); + sb.append(" "); + + SmartDashboard.putString("Auto Prog", sb.toString()); + SmartDashboard.putNumber("Auto Delay", delay); + } + + public String toString() { + return autonomousPrograms.get(programNumber).getName(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java new file mode 100644 index 0000000..6317f76 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Chimney.java @@ -0,0 +1,70 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.RangeFinder_GP2D120; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Chimney extends SubsystemBase { + private final VictorSPX chimneyTalon = new VictorSPX(Constants.Talon.CHIMNEY_ROLLER); + private final RangeFinder_GP2D120 frontIR = new RangeFinder_GP2D120(1, 0); + private final RangeFinder_GP2D120 middleIR = new RangeFinder_GP2D120(2, 0); + + /** + * Creates a new Chimney. + */ + public Chimney() { + chimneyTalon.setNeutralMode(NeutralMode.Coast); + // chimneyTalon.configNominalOutputVoltage(+0.0f, -0.0f); + // chimneyTalon.configPeakOutputVoltage(+12.0, -12.0); + chimneyTalon.configNominalOutputForward(+0.0f); + chimneyTalon.configNominalOutputReverse(0.0); + chimneyTalon.configPeakOutputForward(+12.0); + chimneyTalon.configPeakOutputReverse(-12.0); + + chimneyTalon.setInverted(false); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + updateSmartDashboard(); + monitorTurntableMovement(); + frontIR.periodic(); + middleIR.periodic(); + } + + void updateSmartDashboard() { + SmartDashboard.putNumber("Chimney Speed", chimneyTalon.getMotorOutputPercent()); + // SmartDashboard.putNumber("Chimney Current", chimneyTalon.getStatorCurrent()); + SmartDashboard.putBoolean("Chimney FrontIR", frontIR.isObjectClose()); + SmartDashboard.putBoolean("Chimney MidddleIR", middleIR.isObjectClose()); + } + + void monitorTurntableMovement() { + } + + public void setChimneySpeed(double speed) { + chimneyTalon.set(ControlMode.PercentOutput, speed); + } + + public boolean isBallInFrontOfChimney() { + return frontIR.isObjectClose(); + } + + public boolean isBallInMiddleOfChimney() { + return middleIR.isObjectClose(); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java new file mode 100644 index 0000000..d4c02a8 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Climber.java @@ -0,0 +1,103 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; + +import edu.wpi.first.wpilibj.Solenoid; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Climber extends SubsystemBase { + private static final int MAX_HEIGHT_SOFT_LIMIT = 640000; + private static final int MIN_HEIGHT_SOFT_LIMIT = 5000; + + private final MayhemTalonSRX winchLeft = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_LEFT, + CurrentLimit.LOW_CURRENT); + private final MayhemTalonSRX winchRight = new MayhemTalonSRX(Constants.Talon.CLIMBER_WINCH_RIGHT, + CurrentLimit.LOW_CURRENT); + // private final MayhemTalonSRX walkerLeft = new + // MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_LEFT); + // private final MayhemTalonSRX walkerRight = new + // MayhemTalonSRX(Constants.Talon.CLIMBER_WALKER_RIGHT); + + private final Solenoid pistons = new Solenoid(Constants.Solenoid.CLIMBER_PISTONS); + + /** + * Creates a new Climber. + */ + public Climber() { + + winchLeft.setNeutralMode(NeutralMode.Brake); + winchLeft.configNominalOutputVoltage(+0.0f, -0.0f); + winchLeft.configPeakOutputVoltage(+12.0, -12.0); + winchLeft.setInverted(true); + winchLeft.configForwardSoftLimitThreshold(MAX_HEIGHT_SOFT_LIMIT); + winchLeft.configForwardSoftLimitEnable(true); + winchLeft.configReverseSoftLimitThreshold(MIN_HEIGHT_SOFT_LIMIT); + winchLeft.configReverseSoftLimitEnable(true); + + winchRight.setNeutralMode(NeutralMode.Brake); + winchRight.configNominalOutputVoltage(+0.0f, -0.0f); + winchRight.configPeakOutputVoltage(+12.0, -12.0); + winchRight.setInverted(false); + winchRight.configForwardSoftLimitThreshold(MAX_HEIGHT_SOFT_LIMIT); + winchRight.configForwardSoftLimitEnable(true); + winchRight.configReverseSoftLimitThreshold(MIN_HEIGHT_SOFT_LIMIT); + winchRight.configReverseSoftLimitEnable(true); + + // walkerRight.setNeutralMode(NeutralMode.Brake); + // walkerRight.configNominalOutputVoltage(+0.0f, -0.0f); + // walkerRight.configPeakOutputVoltage(+12.0, -12.0); + + // walkerLeft.setNeutralMode(NeutralMode.Brake); + // walkerLeft.configNominalOutputVoltage(+0.0f, -0.0f); + // walkerLeft.configPeakOutputVoltage(+12.0, -12.0); + } + + public void zero() { + winchLeft.setSelectedSensorPosition(0); + winchRight.setSelectedSensorPosition(0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + SmartDashboard.putNumber("Climber Winch Left", winchLeft.getPosition()); + SmartDashboard.putNumber("Climber Winch Right", winchRight.getPosition()); + // SmartDashboard.putNumber("Climber Walker Left", walkerLeft.getPosition()); + // SmartDashboard.putNumber("Climber Walker Right", walkerRight.getPosition()); + // SmartDashboard.putBoolean("Climber Pistons", pistons.get()); + } + + public void setWinchLeftSpeed(double power) { + winchLeft.set(ControlMode.PercentOutput, power); + } + + public void setWinchRightSpeed(double power) { + winchRight.set(ControlMode.PercentOutput, power); + } + + // public void setWalkerLeftSpeed(double power) { + // walkerLeft.set(ControlMode.Velocity, power); + // } + + // public void setWalkerRightSpeed(double power) { + // walkerRight.set(ControlMode.Velocity, power); + // } + + public void setPistons(boolean b) { + pistons.set(b); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java new file mode 100644 index 0000000..cf926cb --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Drive.java @@ -0,0 +1,828 @@ +package org.mayheminc.robot2020.subsystems; + +import com.kauailabs.navx.frc.*; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.History; + +import edu.wpi.first.wpilibj.*; +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + +//import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +//import org.mayheminc.robot2020.Robot; +//import org.mayheminc.robot2019.RobotMap; +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.Utils; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; + +// TODO: Address all deprecated code masked by @SuppressWarnings("removal") annotations (not just in Drive.java) +@SuppressWarnings("removal") +public class Drive extends SubsystemBase implements PidTunerObject { + + History headingHistory = new History(); + + // Brake modes + public static final boolean BRAKE_MODE = true; + public static final boolean COAST_MODE = false; + + // PID loop variables + private final PIDController m_HeadingPid; + private final PIDHeadingError m_HeadingError; + private final PIDHeadingCorrection m_HeadingCorrection; + private boolean m_HeadingPidPreventWindup = false; + private static final int LOOPS_GYRO_DELAY = 10; + + // Talons + private final MayhemTalonSRX leftFrontTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_LEFT_A, + CurrentLimit.HIGH_CURRENT); + private final MayhemTalonSRX leftRearTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_LEFT_B, + CurrentLimit.HIGH_CURRENT); + private final MayhemTalonSRX rightFrontTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_RIGHT_A, + CurrentLimit.HIGH_CURRENT); + private final MayhemTalonSRX rightRearTalon = new MayhemTalonSRX(Constants.Talon.DRIVE_RIGHT_B, + CurrentLimit.HIGH_CURRENT); + + // Sensors + private AHRS Navx; + + // Driving mode + private final boolean m_speedRacerDriveMode = true; // set by default + + // NavX parameters + private double m_desiredHeading = 0.0; + private boolean m_useHeadingCorrection = true; + private static final double HEADING_PID_P = 0.010; // was 0.007 at GSD; was 0.030 in 2019 for HIGH_GEAR + private static final double HEADING_PID_I = 0.000; // was 0.000 at GSD; was 0.000 in 2019 + private static final double HEADING_PID_D = 0.080; // was 0.080 at GSD; was 0.04 in 2019 + private static final double kToleranceDegreesPIDControl = 0.2; + + // Drive parameters + // pi * diameter * (pulley ratios) / (counts per rev * gearbox reduction) + public static final double DISTANCE_PER_PULSE_IN_INCHES = 3.14 * 5.75 * 36.0 / 42.0 / (2048.0 * 7.56); // corrected + // for 2020 + + private boolean m_closedLoopMode = true; + private final double m_maxWheelSpeed = 18000.0; // should be maximum wheel speed in native units + private static final double CLOSED_LOOP_RAMP_RATE = 0.1; // time from neutral to full in seconds + + private double m_initialWheelDistance = 0.0; + private int m_iterationsSinceRotationCommanded = 0; + private int m_iterationsSinceMovementCommanded = 0; + + private boolean autoAlign = false; + + // Targeting + // private double TARGET_ALIGNED = -0.4; + + /*********************************** + * INITIALIZATION + **********************************************************/ + + public Drive() { + try { + /* Communicate w/navX MXP via the MXP SPI Bus. */ + /* Alternatively: I2C.Port.kMXP, SerialPort.Port.kMXP or SerialPort.Port.kUSB */ + /* + * See http://navx-mxp.kauailabs.com/guidance/selecting-an-interface/ for + * details. + */ + Navx = new AHRS(SPI.Port.kMXP); + Navx.reset(); + } catch (final RuntimeException ex) { + DriverStation.reportError("Error instantiating navX MXP: " + ex.getMessage(), true); + System.out.println("Error loading navx."); + } + + // create a PID Controller that reads the heading error and outputs the heading + // correction. + m_HeadingError = new PIDHeadingError(); + m_HeadingError.m_Error = 0.0; + m_HeadingCorrection = new PIDHeadingCorrection(); + m_HeadingPid = new PIDController(HEADING_PID_P, HEADING_PID_I, HEADING_PID_D, m_HeadingError, + m_HeadingCorrection, 0.020 /* period in seconds */); + m_HeadingPid.setInputRange(-180.0f, 180.0f); + m_HeadingPid.setContinuous(true); // treats the input range as "continuous" with wrap-around + m_HeadingPid.setOutputRange(-.50, .50); // set the maximum power to correct twist + m_HeadingPid.setAbsoluteTolerance(kToleranceDegreesPIDControl); + + // confirm all four drive talons are in coast mode + leftFrontTalon.setNeutralMode(NeutralMode.Coast); + leftRearTalon.setNeutralMode(NeutralMode.Coast); + rightFrontTalon.setNeutralMode(NeutralMode.Coast); + rightRearTalon.setNeutralMode(NeutralMode.Coast); + + this.configTalon(leftFrontTalon); + this.configTalon(leftRearTalon); + this.configTalon(rightFrontTalon); + this.configTalon(rightRearTalon); + + // configure output voltages + leftFrontTalon.configNominalOutputVoltage(+0.0f, -0.0f); + leftRearTalon.configNominalOutputVoltage(+0.0f, -0.0f); + rightFrontTalon.configNominalOutputVoltage(+0.0f, -0.0f); + rightRearTalon.configNominalOutputVoltage(+0.0f, -0.0f); + leftFrontTalon.configPeakOutputVoltage(+12.0, -12.0); + leftRearTalon.configPeakOutputVoltage(+12.0, -12.0); + rightFrontTalon.configPeakOutputVoltage(+12.0, -12.0); + rightRearTalon.configPeakOutputVoltage(+12.0, -12.0); + + // configure current limits + // enabled = true + // 40 = limit (amps) + // 60 = trigger_threshold (amps) + // 0.5 = threshold time(s) + leftFrontTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5)); + leftRearTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5)); + rightFrontTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5)); + rightRearTalon.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 40, 60, 0.5)); + + // set rear talons to follow their respective front talons + leftRearTalon.follow(leftFrontTalon); + rightRearTalon.follow(rightFrontTalon); + + // the left motors move the robot forwards with positive power + // but the right motors are backwards. + leftFrontTalon.setInverted(false); + leftRearTalon.setInverted(false); + rightFrontTalon.setInverted(true); + rightRearTalon.setInverted(true); + + // talon closed loop config + configureDriveTalon(leftFrontTalon); + configureDriveTalon(rightFrontTalon); + } + + private void configTalon(TalonSRX talon) { + talon.configPeakCurrentLimit(60); + talon.configContinuousCurrentLimit(40); + talon.configPeakCurrentDuration(3000); + } + + public void init() { + // reset the NavX + zeroHeadingGyro(0.0); + + // talon closed loop config + configureDriveTalon(leftFrontTalon); + configureDriveTalon(rightFrontTalon); + } + + public void zeroHeadingGyro(final double headingOffset) { + Navx.zeroYaw(); + setHeadingOffset(headingOffset); + + DriverStation.reportError("heading immediately after zero = " + getHeading() + "\n", false); + + m_desiredHeading = 0.0; + + SmartDashboard.putString("Trace", "Zero Heading Gyro"); + + // restart the PID controller loop + resetAndEnableHeadingPID(); + } + + public void initDefaultCommand() { + // setDefaultCommand(new SpeedRacerDrive()); + } + + private void configureDriveTalon(final MayhemTalonSRX talon) { + final double wheelP = 0.020; + final double wheelI = 0.000; + final double wheelD = 0.200; + final double wheelF = 0.060; + + talon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); + + talon.configNominalOutputVoltage(+0.0f, -0.0f); + talon.configPeakOutputVoltage(+12.0, -12.0); + + talon.config_kP(0, wheelP, 0); + talon.config_kI(0, wheelI, 0); + talon.config_kD(0, wheelD, 0); + talon.config_kF(0, wheelF, 0); + talon.configClosedloopRamp(CLOSED_LOOP_RAMP_RATE); // specify minimum time for neutral to full in seconds + + DriverStation.reportError("setWheelPIDF: " + wheelP + " " + wheelI + " " + wheelD + " " + wheelF + "\n", false); + } + + /** + * Set the "Brake Mode" behavior on the drive talons when "in neutral" + * + * @param brakeMode - true for "brake in neutral" and false for "coast in + * neutral" + */ + public void setBrakeMode(final boolean brakeMode) { + + final NeutralMode mode = (brakeMode) ? NeutralMode.Brake : NeutralMode.Coast; + + leftFrontTalon.setNeutralMode(mode); + leftRearTalon.setNeutralMode(mode); + rightFrontTalon.setNeutralMode(mode); + rightRearTalon.setNeutralMode(mode); + } + + // *********************** CLOSED-LOOP MODE ******************************** + + public void toggleClosedLoopMode() { + if (!m_closedLoopMode) { + setClosedLoopMode(); + } else { + setOpenLoopMode(); + } + } + + public void setClosedLoopMode() { + m_closedLoopMode = true; + } + + public void setOpenLoopMode() { + m_closedLoopMode = false; + } + + // ********************* ENCODER-GETTERS ************************************ + + public int getRightEncoder() { + return rightFrontTalon.getSelectedSensorPosition(0); + } + + public int getLeftEncoder() { + return leftFrontTalon.getSelectedSensorPosition(0); + } + + // speed is in inches per second + public double getRightSpeed() { + return rightFrontTalon.getSelectedSensorVelocity(0); + } + + public double getLeftSpeed() { + return leftFrontTalon.getSelectedSensorVelocity(0); + } + + // *************************** GYRO ******************************************* + + public double calculateHeadingError(final double Target) { + final double currentHeading = getHeading(); + double error = Target - currentHeading; + error = error % 360.0; + if (error > 180.0) { + error -= 360.0; + } + return error; + } + + public boolean getHeadingCorrectionMode() { + return m_useHeadingCorrection; + } + + public void setHeadingCorrectionMode(final boolean useHeadingCorrection) { + m_useHeadingCorrection = useHeadingCorrection; + } + + private void resetAndEnableHeadingPID() { + m_HeadingPid.reset(); + m_HeadingPid.enable(); + } + + static private final double STATIONARY = 0.1; + static private double m_prevLeftDistance = 0.0; + static private double m_prevRightDistance = 0.0; + + public boolean isStationary() { + final double leftDistance = getLeftEncoder(); + final double rightDistance = getRightEncoder(); + + final double leftDelta = Math.abs(leftDistance - m_prevLeftDistance); + final double rightDelta = Math.abs(rightDistance - m_prevRightDistance); + + m_prevLeftDistance = leftDistance; + m_prevRightDistance = rightDistance; + + return leftDelta < STATIONARY && rightDelta < STATIONARY; + } + + private int LoopCounter = 0; + + public void displayGyroInfo() { + SmartDashboard.putNumber("Robot Heading", Utils.twoDecimalPlaces(getHeading())); + SmartDashboard.putNumber("Robot Roll", Utils.twoDecimalPlaces(this.getRoll())); + SmartDashboard.putNumber("Robot Pitch", Utils.twoDecimalPlaces(this.getPitch())); + SmartDashboard.putNumber("Loop Counter", LoopCounter++); + } + + private double m_headingOffset = 0.0; + + public void setHeadingOffset(final double arg_offset) { + m_headingOffset = arg_offset; + } + + public double getHeading() { + return Navx.getYaw() + m_headingOffset; + } + + // the Navx is installed sidways with reference to the front of the robot. + public double getRoll() { + return Navx.getPitch(); + } + + // the Navx is installed sidways with reference to the front of the robot. + public double getPitch() { + return Navx.getRoll(); + } + + public double getDesiredHeading() { + return m_desiredHeading; + } + + // ****************** SETTING POWER TO MOTORS ******************** + + public void resetNavXDisplacement() { + Navx.resetDisplacement(); + } + + public void stop() { + setMotorPower(0, 0); + } + + private void setMotorPower(double leftPower, double rightPower) { + if (rightPower > 1.0) { + rightPower = 1.0; + } + if (rightPower < -1.0) { + rightPower = -1.0; + } + + if (leftPower > 1.0) { + leftPower = 1.0; + } + if (leftPower < -1.0) { + leftPower = -1.0; + } + + if (m_closedLoopMode) { + rightFrontTalon.set(ControlMode.Velocity, rightPower * m_maxWheelSpeed); + leftFrontTalon.set(ControlMode.Velocity, leftPower * m_maxWheelSpeed); + } else { + rightFrontTalon.set(ControlMode.PercentOutput, rightPower); + leftFrontTalon.set(ControlMode.PercentOutput, leftPower); + } + } + + PowerDistributionPanel pdp = new PowerDistributionPanel(); + + /** + * updateSdbPdp Update the Smart Dashboard with the Power Distribution Panel + * currents. + */ + public void updateSdbPdp() { + double lf; + double rf; + double lb; + double rb; + final double fudgeFactor = 0.0; + + lf = pdp.getCurrent(Constants.PDP.DRIVE_LEFT_A) - fudgeFactor; + rf = pdp.getCurrent(Constants.PDP.DRIVE_LEFT_B) - fudgeFactor; + lb = pdp.getCurrent(Constants.PDP.DRIVE_RIGHT_A) - fudgeFactor; + rb = pdp.getCurrent(Constants.PDP.DRIVE_RIGHT_B) - fudgeFactor; + + SmartDashboard.putNumber("Left Front I", lf); + SmartDashboard.putNumber("Right Front I", rf); + SmartDashboard.putNumber("Left Back I", lb); + SmartDashboard.putNumber("Right Back I", rb); + + } + + /* + * This method allows one to drive in "Tank Drive Mode". Tank drive mode uses + * the left side of the joystick to control the left side of the robot, whereas + * the right side of the joystick controls the right side of the robot. + */ + public void tankDrive(double leftSideThrottle, double rightSideThrottle) { + // if (leftSideThrottle >= 0.0) { + // leftSideThrottle = (leftSideThrottle * leftSideThrottle); // squaring inputs + // increases fine control + // } else { + // leftSideThrottle = -(leftSideThrottle * leftSideThrottle); // preserves the + // sign while squaring negative + // // values + // } + + // if (rightSideThrottle >= 0.0) { + // rightSideThrottle = (rightSideThrottle * rightSideThrottle); + // } else { + // rightSideThrottle = -(rightSideThrottle * rightSideThrottle); + // } + + setMotorPower(leftSideThrottle, rightSideThrottle); + } + + public void setAutoAlignTrue() { + autoAlign = true; + // reset the PID controller loop for steering now that we are auto-aligning + resetAndEnableHeadingPID(); + // Robot.lights.set(LedPatternFactory.autoAlignTrying); + } + + public void setAutoAlignFalse() { + autoAlign = false; + resetAndEnableHeadingPID(); + // Robot.lights.set(LedPatternFactory.autoAlignGotIt); + } + + public void speedRacerDrive(double throttle, double rawSteeringX, boolean quickTurn) { + speedRacerDriveNew(throttle, rawSteeringX, quickTurn); + } + + public void speedRacerDriveNew(double throttle, double rawSteeringX, boolean quickTurn) { + double leftPower, rightPower; + double rotation = 0; + final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast. + + int throttleSign; + if (throttle >= 0.0) { + throttleSign = 1; + } else { + throttleSign = -1; + } + + // check for if steering input is essentially zero for "DriveStraight" + // functionality + if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) { + // no turn being commanded, drive straight or stay still + m_iterationsSinceRotationCommanded++; + if ((-0.01 < throttle) && (throttle < 0.01)) { + // no motion commanded, stay still + m_iterationsSinceMovementCommanded++; + rotation = 0.0; + m_desiredHeading = getHeading(); // whenever motionless, set desired heading to current heading + // reset the PID controller loop now that we have a new desired heading + resetAndEnableHeadingPID(); + } else { + // driving straight + if ((m_iterationsSinceRotationCommanded == LOOPS_GYRO_DELAY) + || (m_iterationsSinceMovementCommanded >= LOOPS_GYRO_DELAY)) { + // exactly LOOPS_GYRO_DELAY iterations with no commanded turn, + // or haven't had movement commanded for longer than LOOPS_GYRO_DELAY, + // so we want to take steps to preserve our current heading hereafter + + // get current heading as desired heading + m_desiredHeading = getHeading(); + + // reset the PID controller loop now that we have a new desired heading + resetAndEnableHeadingPID(); + rotation = 0.0; + } else if (m_iterationsSinceRotationCommanded < LOOPS_GYRO_DELAY) { + // not long enough since we were last turning, + // just drive straight without special heading maintenance + rotation = 0.0; + } else if (m_iterationsSinceRotationCommanded > LOOPS_GYRO_DELAY) { + // after more then LOOPS_GYRO_DELAY iterations since commanded turn, + // maintain the target heading + rotation = maintainHeading(); + } + m_iterationsSinceMovementCommanded = 0; + } + // driveStraight code benefits from "spin" behavior when needed + leftPower = throttle + rotation; + rightPower = throttle - rotation; + } else { + // commanding a turn, reset iterationsSinceRotationCommanded + m_iterationsSinceRotationCommanded = 0; + m_iterationsSinceMovementCommanded = 0; + if (quickTurn) { + // want a high-rate turn (also allows "spin" behavior) + // power to each wheel is a combination of the throttle and rotation + rotation = rawSteeringX * throttleSign * QUICK_TURN_GAIN; + leftPower = throttle + rotation; + rightPower = throttle - rotation; + } else { + // want a standard rate turn (scaled by the throttle) + if (rawSteeringX >= 0.0) { + // turning to the right, derate the right power by turn amount + // note that rawSteeringX is positive in this portion of the "if" + leftPower = throttle; + rightPower = throttle * (1.0 - Math.abs(rawSteeringX)); + } else { + // turning to the left, derate the left power by turn amount + // note that rawSteeringX is negative in this portion of the "if" + leftPower = throttle * (1.0 - Math.abs(rawSteeringX)); + rightPower = throttle; + } + } + } + setMotorPower(leftPower, rightPower); + } + + public void speedRacerDriveOld(final double throttle, final double rawSteeringX, final boolean quickTurn) { + double leftPower, rightPower; + double rotation = 0; + final double adjustedSteeringX = rawSteeringX * throttle; + final double QUICK_TURN_GAIN = 0.55; // 2019: .75. 2020: .75 was too fast. + final double STD_TURN_GAIN = 0.9; // 2019: 1.5. 2020: 1.5 was too fast// the driver wants the non-quick turn + // turning a little more responsive. + + int throttleSign; + if (throttle >= 0.0) { + throttleSign = 1; + } else { + throttleSign = -1; + } + if (autoAlign) { + // DriverStation.reportWarning("Auto align was called in drive base", false); + + // Use the targeting code to get the desired heading + // m_desiredHeading = Robot.targeting.desiredHeading(); + + // Use the heading PID to provide the rotation input + rotation = maintainHeading(); + + } else { + // not using camera targeting right now + + // check for if steering input is essentially zero + if ((-0.01 < rawSteeringX) && (rawSteeringX < 0.01)) { + // no turn being commanded, drive straight or stay still + m_iterationsSinceRotationCommanded++; + if ((-0.01 < throttle) && (throttle < 0.01)) { + // no motion commanded, stay still + m_iterationsSinceMovementCommanded++; + rotation = 0.0; + m_desiredHeading = getHeading(); // whenever motionless, set desired heading to current heading + // reset the PID controller loop now that we have a new desired heading + resetAndEnableHeadingPID(); + } else { + // driving straight + if ((m_iterationsSinceRotationCommanded == LOOPS_GYRO_DELAY) + || (m_iterationsSinceMovementCommanded >= LOOPS_GYRO_DELAY)) { + // exactly LOOPS_GYRO_DELAY iterations with no commanded turn, + // or haven't had movement commanded for longer than LOOPS_GYRO_DELAY, + // so we want to take steps to preserve our current heading hereafter + + // get current heading as desired heading + m_desiredHeading = getHeading(); + + // reset the PID controller loop now that we have a new desired heading + resetAndEnableHeadingPID(); + rotation = 0.0; + } else if (m_iterationsSinceRotationCommanded < LOOPS_GYRO_DELAY) { + // not long enough since we were last turning, + // just drive straight without special heading maintenance + rotation = 0.0; + } else if (m_iterationsSinceRotationCommanded > LOOPS_GYRO_DELAY) { + // after more then LOOPS_GYRO_DELAY iterations since commanded turn, + // maintain the target heading + rotation = maintainHeading(); + } + m_iterationsSinceMovementCommanded = 0; + } + } else { + // commanding a turn, reset iterationsSinceRotationCommanded + m_iterationsSinceRotationCommanded = 0; + m_iterationsSinceMovementCommanded = 0; + + if (quickTurn) { + // want a high-rate turn (also allows "spin" behavior) + rotation = rawSteeringX * throttleSign * QUICK_TURN_GAIN; + } else { + // want a standard rate turn (scaled by the throttle) + rotation = adjustedSteeringX * STD_TURN_GAIN; // set the turn to the throttle-adjusted steering + // input + + } + } + } + // power to each wheel is a combination of the throttle and rotation + leftPower = throttle + rotation; + rightPower = throttle - rotation; + setMotorPower(leftPower, rightPower); + } + + public int stage = 0; + public final double DELAY = 4; + public double timerStart = Timer.getFPGATimestamp(); + + public boolean selfTest() { + return false; + } + + /** + * The headings are from -180 to 180. The CurrentHeading is the current robot + * orientation. The TargetHeading is where we want the robot to face. + * + * e.g. Current = 0, Target = 10, error = 10 Current = 180, Target = -170, error + * = 10 (we need to turn 10 deg Clockwise to get to -170) Current = -90, Target + * = 180, error = -90 (we need to turn 90 deg Counter-Clockwise to get to 180) + * Current = 10, target = -10, error = -20 (we need to turn Counterclockwise -20 + * deg) + * + * @param CurrentHeading + * @param TargetHeading + * @return + */ + private double maintainHeading() { + double headingCorrection = 0.0; + + // below line is essential to let the Heading PID know the current heading error + m_HeadingError.m_Error = m_desiredHeading - getHeading(); + + if (m_useHeadingCorrection) { + headingCorrection = -m_HeadingCorrection.HeadingCorrection; + } else { + headingCorrection = 0.0; + } + + // check for major heading changes and take action to prevent + // integral windup if there is a major heading error + // TODO: In 2020, this code was causing "wandering" with non-zero HEADING_PID_I. + // Worked around issue by setting HEADING_PID_I = 0 + if (Math.abs(m_HeadingError.m_Error) > 10.0) { + if (!m_HeadingPidPreventWindup) { + m_HeadingPid.setI(0.0); + resetAndEnableHeadingPID(); + m_HeadingPidPreventWindup = true; + } + } else { + m_HeadingPidPreventWindup = false; + m_HeadingPid.setI(HEADING_PID_I); + } + + return headingCorrection; + } + + public void rotate(final double RotateX) { + m_desiredHeading += RotateX; + if (m_desiredHeading > 180) { + m_desiredHeading -= 360; + } + if (m_desiredHeading < -180) { + m_desiredHeading += 360; + } + m_iterationsSinceRotationCommanded = LOOPS_GYRO_DELAY + 1; // hack so speedracerdrive maintains heading + m_iterationsSinceMovementCommanded = 0; + } + + public void rotateToHeading(final double desiredHeading) { + m_desiredHeading = desiredHeading; + } + + // **********************************************DISPLAY**************************************************** + + @Override + public void periodic() { + updateSmartDashboard(); + } + + public void updateSmartDashboard() { + displayGyroInfo(); + + SmartDashboard.putBoolean("In Autonomous", DriverStation.getInstance().isAutonomous()); + SmartDashboard.putNumber("Battery Voltage", RobotController.getBatteryVoltage()); + + // ***** KBS: Uncommenting below, as it takes a LONG time to get PDP values + // updateSdbPdp(); + + int matchnumber = DriverStation.getInstance().getMatchNumber(); + DriverStation.MatchType MatchType = DriverStation.getInstance().getMatchType(); + SmartDashboard.putString("matchInfo", "" + MatchType + '_' + matchnumber); + SmartDashboard.putNumber("Left Front Encoder Counts", leftFrontTalon.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Front Encoder Counts", rightFrontTalon.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Left Rear Encoder Counts", leftRearTalon.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Rear Encoder Counts", rightRearTalon.getSelectedSensorPosition(0)); + + // Note: getSpeed() returns ticks per 0.1 seconds + SmartDashboard.putNumber("Left Encoder Speed", leftFrontTalon.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Right Encoder Speed", rightFrontTalon.getSelectedSensorVelocity(0)); + + // To convert ticks per 0.1 seconds into feet per second + // a - multiply be 10 (tenths of second per second) + // b - divide by 12 (1 foot per 12 inches) + // c - multiply by distance (in inches) per pulse + SmartDashboard.putNumber("Left Speed (fps)", + leftFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE_IN_INCHES); + SmartDashboard.putNumber("Right Speed (fps)", + rightFrontTalon.getSelectedSensorVelocity(0) * 10 / 12 * DISTANCE_PER_PULSE_IN_INCHES); + + SmartDashboard.putNumber("Left Talon Output Voltage", leftFrontTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("Right Talon Output Voltage", rightFrontTalon.getMotorOutputVoltage()); + + SmartDashboard.putNumber("LF Falcon Supply Current", leftFrontTalon.getSupplyCurrent()); + SmartDashboard.putNumber("LR Falcon Supply Current", leftRearTalon.getSupplyCurrent()); + SmartDashboard.putNumber("RF Falcon Supply Current", rightFrontTalon.getSupplyCurrent()); + SmartDashboard.putNumber("RR Falcon Supply Current", rightRearTalon.getSupplyCurrent()); + + SmartDashboard.putBoolean("Closed Loop Mode", m_closedLoopMode); + SmartDashboard.putBoolean("Speed Racer Drive Mode", m_speedRacerDriveMode); + + SmartDashboard.putBoolean("Heading Correction Mode", m_useHeadingCorrection); + SmartDashboard.putNumber("Heading Desired", m_desiredHeading); + SmartDashboard.putNumber("Heading Error", m_HeadingError.m_Error); + SmartDashboard.putNumber("Heading Correction", -m_HeadingCorrection.HeadingCorrection); + + // SmartDashboard.putNumber("Joystick Drive Throttle", + // Robot.oi.driveThrottle()); + // SmartDashboard.putNumber("Joystick SteeringX", Robot.oi.steeringX()); + + // determine currentSpeed and display it + // double currentSpeed = getLeftSpeed() > getRightSpeed() ? getLeftSpeed() : + // getRightSpeed(); + + // display current speed to driver + // SmartDashboard.putNumber("Current Speed", currentSpeed); + } + + private static final double CAMERA_LAG = 0.150; // was .200; changing to .150 at CMP + + public void updateHistory() { + final double now = Timer.getFPGATimestamp(); + headingHistory.add(now, getHeading()); + } + + public double getHeadingForCapturedImage() { + final double now = Timer.getFPGATimestamp(); + final double indexTime = now - CAMERA_LAG; + return headingHistory.getAzForTime(indexTime); + } + + /** + * Start a distance travel + */ + public void saveInitialWheelDistance() { + m_initialWheelDistance = (getLeftEncoder() + getRightEncoder()) / 2; + } + + /** + * Calculate the distance travelled. Return the second shortest distance. If a + * wheel is floating, it will have a larger value - ignore it. If a wheel is + * stuck, it will have a small value + * + * @return + */ + public double getWheelDistance() { + final double dist = (getLeftEncoder() + getRightEncoder()) / 2; + return dist - m_initialWheelDistance; + } + + // NOTE the difference between rotateToHeading(...) and goToHeading(...) + public void setDesiredHeading(final double desiredHeading) { + m_desiredHeading = desiredHeading; + m_iterationsSinceRotationCommanded = LOOPS_GYRO_DELAY + 1; + m_iterationsSinceMovementCommanded = 0; + + // reset the heading control loop for the new heading + resetAndEnableHeadingPID(); + } + + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return leftFrontTalon.getP(); + } + + @Override + public double getI() { + return leftFrontTalon.getI(); + } + + @Override + public double getD() { + return leftFrontTalon.getD(); + } + + @Override + public double getF() { + return leftFrontTalon.getF(); + + } + + @Override + public void setP(double d) { + leftFrontTalon.config_kP(0, d, 0); + rightFrontTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + leftFrontTalon.config_kI(0, d, 0); + rightFrontTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + leftFrontTalon.config_kD(0, d, 0); + rightFrontTalon.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + leftFrontTalon.config_kF(0, d, 0); + rightFrontTalon.config_kF(0, d, 0); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Feeder.java b/src/main/java/org/mayheminc/robot2020/subsystems/Feeder.java new file mode 100644 index 0000000..3e22bbf --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Feeder.java @@ -0,0 +1,47 @@ +package org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.Constants; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Feeder extends SubsystemBase { + private final VictorSPX feederTalon = new VictorSPX(Constants.Talon.SHOOTER_FEEDER); + + /** + * Creates a new Feeder. + */ + public Feeder() { + + configureTalon(); + } + + public void init() { + configureTalon(); } + + private void configureTalon() { + feederTalon.setNeutralMode(NeutralMode.Brake); + feederTalon.configNominalOutputForward(+0.0f); + feederTalon.configNominalOutputReverse(0.0); + feederTalon.configPeakOutputForward(+12.0); + feederTalon.configPeakOutputReverse(-6.0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + UpdateDashboard(); + } + + private void UpdateDashboard() { + SmartDashboard.putNumber("Shooter feeder speed", feederTalon.getMotorOutputVoltage()); + } + + public void setSpeed(double pos) { + feederTalon.set(ControlMode.PercentOutput, pos); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java new file mode 100644 index 0000000..881fcff --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Hood.java @@ -0,0 +1,139 @@ +package org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.Constants; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.mayheminc.util.MayhemTalonSRX; +// import org.mayheminc.util.PidTuner; +import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; + +public class Hood extends SubsystemBase implements PidTunerObject { + private final MayhemTalonSRX hoodTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_HOOD, CurrentLimit.LOW_CURRENT); + + private final static int MIN_POSITION = 0; + private final static int MAX_POSITION = 8000; + private final static double POSITION_TOLERANCE = 100.0; + + public final static double STARTING_POSITION = 0; + public final static double TARGET_ZONE_POSITION = 5000; + public final static double CLOSE_SHOOTING_POSITION = 0; + public final static double INITIATION_LINE_POSITION = 5000; + public final static double TRENCH_MID_POSITION = 8000; + + private double m_desiredPosition = 0.0; + + /** + * Creates a new Hood. + */ + public Hood() { + configureTalon(); + } + + public void init() { + configureTalon(); + } + + private void configureTalon() { + hoodTalon.config_kP(0, 10.0, 0); + hoodTalon.config_kI(0, 0.0, 0); + hoodTalon.config_kD(0, 0.0, 0); + hoodTalon.config_kF(0, 0.0, 0); + + hoodTalon.changeControlMode(ControlMode.Position); + hoodTalon.setNeutralMode(NeutralMode.Coast); + hoodTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + hoodTalon.configNominalOutputVoltage(+0.0f, -0.0f); + hoodTalon.configPeakOutputVoltage(+12.0, -12.0); + hoodTalon.setInverted(false); + hoodTalon.setSensorPhase(false); + + hoodTalon.configForwardSoftLimitThreshold(MAX_POSITION); + hoodTalon.configForwardSoftLimitEnable(true); + hoodTalon.configReverseSoftLimitThreshold(MIN_POSITION); + hoodTalon.configReverseSoftLimitEnable(true); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + UpdateDashboard(); + } + + private void UpdateDashboard() { + SmartDashboard.putNumber("Shooter Hood Pos", hoodTalon.getPosition()); + SmartDashboard.putNumber("Shooter Hood Setpoint", m_desiredPosition); + } + + public void zero() { + hoodTalon.setPosition(0); + m_desiredPosition = 0.0; + hoodTalon.set(ControlMode.PercentOutput, 0.0); + } + + public void setPosition(double pos) { + m_desiredPosition = pos; + hoodTalon.set(ControlMode.Position, m_desiredPosition); + } + + public boolean isAtPosition() { + return (Math.abs(getPosition() - m_desiredPosition) < POSITION_TOLERANCE); + } + + public double getPosition() { + return hoodTalon.getPosition(); + } + + public void setVBus(double d) { + hoodTalon.set(ControlMode.PercentOutput, d); + } + + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return hoodTalon.getP(); + } + + @Override + public double getI() { + return hoodTalon.getI(); + } + + @Override + public double getD() { + return hoodTalon.getD(); + } + + @Override + public double getF() { + return hoodTalon.getF(); + + } + + @Override + public void setP(double d) { + hoodTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + hoodTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + hoodTalon.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + hoodTalon.config_kF(0, d, 0); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java new file mode 100644 index 0000000..46a2e43 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Intake.java @@ -0,0 +1,241 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.VictorSPX; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Intake extends SubsystemBase implements PidTunerObject { + + private final int PIVOT_CLOSE_ENOUGH = 50; + private final VictorSPX rollersTalon = new VictorSPX(Constants.Talon.INTAKE_ROLLERS); + private final MayhemTalonSRX pivotTalon = new MayhemTalonSRX(Constants.Talon.INTAKE_PIVOT, CurrentLimit.LOW_CURRENT); + private static final int PIVOT_ZERO_POSITION = 950; + public static final double PIVOT_UP = PIVOT_ZERO_POSITION; + public static final double PIVOT_SHOOTING = 100.0; + public static final double PIVOT_DOWN = -240.0; + + private static final double HORIZONTAL_HOLD_OUTPUT = 0.00; + private static final double MAX_PID_MOVEMENT_TIME_SEC = 10.0; + + enum PivotMode { + MANUAL_MODE, PID_MODE, + }; + + private PivotMode m_mode = PivotMode.MANUAL_MODE; + private boolean m_isMoving; + private double m_desiredRollersPower = 0.0; + private double m_targetPosition; + private double m_feedForward; + private Timer m_pidTimer = new Timer(); + + /** + * Creates a new Intake. + */ + public Intake() { + rollersTalon.setNeutralMode(NeutralMode.Coast); + rollersTalon.configNominalOutputForward(+0.0f); + rollersTalon.configNominalOutputReverse(0.0); + rollersTalon.configPeakOutputForward(+12.0); + rollersTalon.configPeakOutputReverse(-12.0); + + configPivotMotor(pivotTalon); + } + + void configPivotMotor(MayhemTalonSRX motor) { + // initial calcs for computing kP... + // If we want 50% power when at the full extreme, + // Full extreme is 900 ticks + // kP = (0.5 * 1023) / 900 = 0.568 + motor.config_kP(0, 1.5, 0); // based upon Robert's initial calcs, above + + // typical value of about 1/100 of kP for starting tuning + motor.config_kI(0, 0.0, 0); + + // typical value of about 10x to 100x of kP for starting tuning + motor.config_kD(0, 50.0, 0); + // motor.config_kD(0, 0.575, 0); + + // practically always set kF to 0 for position control + // for things like gravity compensation, use the "arbitrary feed forward" that + // can be specified with the "4-parameter" TalonSRX.set() method + motor.config_kF(0, 0.0, 0); + + motor.setNeutralMode(NeutralMode.Coast); + motor.setInverted(false); + // in general, sensor phase inversion needed for gearboxes which reverse sensor + // direction due to odd number of stages + motor.setSensorPhase(true); + motor.configNominalOutputVoltage(+0.0f, -0.0f); + motor.configPeakOutputVoltage(+12.0, -12.0); + motor.configClosedloopRamp(0.05); // limit neutral to full to 0.05 seconds + motor.configMotionCruiseVelocity(30); // estimate 30 ticks max + motor.configMotionAcceleration(60); // acceleration of 2x velocity allows cruise to be attained in 1/2 + // second + motor.setFeedbackDevice(FeedbackDevice.QuadEncoder); + } + + public void zero() { + pivotTalon.setEncPosition(PIVOT_ZERO_POSITION); + } + + /** + * Negative for intake. Positive for spit. + * + * @param power + */ + public void setRollers(double power) { + m_desiredRollersPower = power; + // rollersTalon.set(ControlMode.PercentOutput, power); + } + + public void setPivot(Double b) { + m_targetPosition = b; + m_mode = PivotMode.PID_MODE; + m_isMoving = true; + + m_pidTimer.start(); + } + + public boolean isPivotAtPosition() { + // return !m_isMoving; + + // the intake motor is broken. + // the intake is now gravity fed + return true; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + updateSmartDashBoard(); + updateSensorPosition(); + updatePivotPower(); + updateRollersPower(); + } + + private void updatePivotPower() { + + if (m_mode == PivotMode.PID_MODE) { + // if the pivot is close enough or it has been on too long, turn the motor on + // gently downards + if ((Math.abs(pivotTalon.getPosition() - m_targetPosition) < PIVOT_CLOSE_ENOUGH) + || m_pidTimer.hasPeriodPassed(Intake.MAX_PID_MOVEMENT_TIME_SEC)) { + m_isMoving = false; + + // if the current position is closer to PIVOT UP than PIVOT DOWN, apply a little + // positive power. + if (pivotTalon.getPosition() > (PIVOT_UP / 2.0)) { + setPivotVBus(+0.05); + } else { // we are close to the PIVOT DOWN, so apply a little negative power. + setPivotVBus(-0.10); + } + } else { + pivotTalon.set(ControlMode.Position, m_targetPosition, DemandType.ArbitraryFeedForward, m_feedForward); + } + } + } + + void updateRollersPower() { + // if (pivotTalon.getPosition() > (PIVOT_UP / 2.0)) { + // // turn off the rollers automatically if the pivot is too high + // rollersTalon.set(ControlMode.PercentOutput, 0.0); + // } else { + // set the rollers as requested if the pivot is low enough + rollersTalon.set(ControlMode.PercentOutput, m_desiredRollersPower); + // } + } + + void updateSensorPosition() { + int m_currentPosition = pivotTalon.getPosition(); + double m_angleInDegrees = positionToDegrees(m_currentPosition); + double m_angleInRadians = Math.toRadians(m_angleInDegrees); + + // get a range of -1 to 1 to multiply by feedforward. + // when in horizontal forward position, value should be 1 + // when in vertical up or down position, value should be 0 + // when in horizontal backward position, value should be -1 + double m_gravityCompensation = Math.cos(m_angleInRadians); + + // HORIZONTAL_HOLD_OUTPUT is the minimum power required to hold the arm up when + // horizontal + // this is a range of -1.0 to 1.0 (%vbus), determined empirically + m_feedForward = m_gravityCompensation * HORIZONTAL_HOLD_OUTPUT; + } + + private double positionToDegrees(int pos) { + return pos / 10; // 900 encoder ticks per 90 degrees + } + + public void updateSmartDashBoard() { + SmartDashboard.putNumber("Intake Position", pivotTalon.getPosition()); + SmartDashboard.putNumber("Intake Target", m_targetPosition); + SmartDashboard.putNumber("Intake FeedForward", m_feedForward); + + SmartDashboard.putBoolean("Intake Is Moving", m_isMoving); + SmartDashboard.putBoolean("Intake PID Mode", (m_mode == PivotMode.PID_MODE)); + SmartDashboard.putNumber("Intake Rollers", rollersTalon.getMotorOutputPercent()); + } + + public void setPivotVBus(double VBus) { + pivotTalon.set(ControlMode.PercentOutput, VBus); + m_mode = PivotMode.MANUAL_MODE; + } + + @Override + public double getP() { + return this.pivotTalon.getP(); + } + + @Override + public double getI() { + return this.pivotTalon.getI(); + } + + @Override + public double getD() { + return this.pivotTalon.getD(); + } + + @Override + public double getF() { + return this.pivotTalon.getF(); + } + + @Override + public void setP(double d) { + this.pivotTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + this.pivotTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + this.pivotTalon.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + this.pivotTalon.config_kF(0, d, 0); + } +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java new file mode 100644 index 0000000..bcf782e --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingCorrection.java @@ -0,0 +1,19 @@ +package org.mayheminc.robot2020.subsystems; +import edu.wpi.first.wpilibj.PIDOutput; + +/** + * + * @author user This class holds the correction that is calculated by the PID + * Controller. + */ +@SuppressWarnings("removal") +public class PIDHeadingCorrection implements PIDOutput { + + public double HeadingCorrection = 0.0; + + @Override + public void pidWrite(double output) { + HeadingCorrection = output; + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java new file mode 100644 index 0000000..8df40c6 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/PIDHeadingError.java @@ -0,0 +1,29 @@ +package org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; + +/** + * + * @author user This is a class to hold the Heading error of the drive. + */ +@SuppressWarnings("removal") +public class PIDHeadingError implements PIDSource { + + @Override + public void setPIDSourceType(PIDSourceType pidSource) { + + } + + @Override + public PIDSourceType getPIDSourceType() { + return PIDSourceType.kDisplacement; + } + + @Override + public double pidGet() { + return m_Error; + } + + public double m_Error = 0.0; +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java b/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java new file mode 100644 index 0000000..83121bd --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Revolver.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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 org.mayheminc.robot2020.subsystems; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; + +public class Revolver extends SubsystemBase { + private final MayhemTalonSRX revolverTalon = new MayhemTalonSRX(Constants.Talon.REVOLVER_TURNTABLE, + CurrentLimit.LOW_CURRENT); + + /** + * Creates a new Revolver. + */ + public Revolver() { + ConfigureTalon(revolverTalon); + revolverTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + } + + private void ConfigureTalon(MayhemTalonSRX talon) { + talon.setNeutralMode(NeutralMode.Coast); + talon.configNominalOutputVoltage(+0.0f, -0.0f); + talon.configPeakOutputVoltage(+12.0, -12.0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + updateSmartDashboard(); + } + + void updateSmartDashboard() { + SmartDashboard.putNumber("Revolver Speed", revolverTalon.getSpeed()); + } + + public void setRevolverSpeed(double speed) { + revolverTalon.set(ControlMode.PercentOutput, speed); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java new file mode 100644 index 0000000..15e2fd2 --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/ShooterWheel.java @@ -0,0 +1,203 @@ +package org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.Constants; +// import org.mayheminc.robot2020.RobotContainer; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; + +public class ShooterWheel extends SubsystemBase implements PidTunerObject { + private final MayhemTalonSRX shooterWheelLeft = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_LEFT, + CurrentLimit.HIGH_CURRENT); + private final MayhemTalonSRX shooterWheelRight = new MayhemTalonSRX(Constants.Talon.SHOOTER_WHEEL_RIGHT, + CurrentLimit.HIGH_CURRENT); + + // private final double MAX_SPEED_RPM = 5760.0; + private final double TALON_TICKS_PER_REV = 2048.0; + private final double SECONDS_PER_MINUTE = 60.0; + private final double HUNDRED_MS_PER_SECOND = 10.0; + + public static final double IDLE_SPEED = 1000.0; + public static final double CLOSE_SHOOTING_SPEED = 4000.0; + public static final double INITIATION_LINE_SPEED = 4500.0; + public static final double TRENCH_FRONT_SPEED = 3400.0; + public static final double MAX_SPEED_RPM = 5000; + + double m_targetSpeedRPM; + + // Note: for ease of thinking, 1 RPM =~ 3.4 native units for the shooter + double convertRpmToTicksPer100ms(double rpm) { + return rpm / SECONDS_PER_MINUTE * TALON_TICKS_PER_REV / HUNDRED_MS_PER_SECOND; + } + + // Note: 3.413 native units =~ 1.0 RPM for the shooter + double convertTicksPer100msToRPM(double ticks) { + return ticks * HUNDRED_MS_PER_SECOND / TALON_TICKS_PER_REV * SECONDS_PER_MINUTE; + } + + /** + * Creates a new Shooter. + */ + public ShooterWheel() { + configureWheelFalcons(); + } + + public void init() { + configureWheelFalcons(); + setSpeedVBus(0.0); + } + + // configure a pair of shooter wheel falcons + private void configureWheelFalcons() { + // most of the configuration is shared for the two Falcons + configureOneWheelFalcon(shooterWheelLeft); + configureOneWheelFalcon(shooterWheelRight); + + // with the exception of one rotating the opposite direction + shooterWheelLeft.setInverted(true); + shooterWheelRight.setInverted(false); + } + + private void configureOneWheelFalcon(MayhemTalonSRX shooterWheelFalcon) { + shooterWheelFalcon.setFeedbackDevice(FeedbackDevice.IntegratedSensor); + shooterWheelFalcon.setNeutralMode(NeutralMode.Coast); + shooterWheelFalcon.configNominalOutputVoltage(+0.0f, -0.0f); + shooterWheelFalcon.configPeakOutputVoltage(+12.0, 0.0); + shooterWheelFalcon.configNeutralDeadband(0.001); // Config neutral deadband to be the smallest possible + + // For PIDF computations, 1023 is interpreted as "full" motor output + // Velocity is in native units of TicksPer100ms. + // 1000rpm =~ 3413 native units. + // P of "3.0" means that full power applied with error of 341 native units = + // 100rpm + // (above also means that 50% power boost applied with error of 50rpm) + shooterWheelFalcon.config_kP(0, 0.1, 0); // previously used 3.0 + shooterWheelFalcon.config_kI(0, 0.0, 0); + shooterWheelFalcon.config_kD(0, 0.0, 0); // CTRE recommends starting at 10x P-gain + shooterWheelFalcon.config_kF(0, 0.046, 0); // 1023.0 / convertRpmToTicksPer100ms(5760), 0); + shooterWheelFalcon.configAllowableClosedloopError(0, 0, 0); // no "neutral" zone around target + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + UpdateDashboard(); + } + + private void UpdateDashboard() { + SmartDashboard.putNumber("Shooter Wheel pos", shooterWheelLeft.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Shooter Wheel speed", shooterWheelLeft.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Shooter Wheel RPM", + convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0))); + + SmartDashboard.putNumber("Shooter Wheel target RPM", m_targetSpeedRPM); + SmartDashboard.putNumber("Shooter Wheel Error", + m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0))); + SmartDashboard.putNumber("Shooter Wheel Voltage", shooterWheelLeft.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Wheel Bus Voltage", shooterWheelLeft.getBusVoltage()); + SmartDashboard.putNumber("Shooter Wheel Current", shooterWheelLeft.getSupplyCurrent()); + + SmartDashboard.putNumber("Shooter Wheel R-pos", shooterWheelRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Shooter Wheel R-speed", shooterWheelRight.getSelectedSensorVelocity(0)); + SmartDashboard.putNumber("Shooter Wheel R-RPM", + convertTicksPer100msToRPM(shooterWheelRight.getSelectedSensorVelocity(0))); + + SmartDashboard.putNumber("Shooter Wheel R-target RPM", m_targetSpeedRPM); + SmartDashboard.putNumber("Shooter Wheel R-Error", + m_targetSpeedRPM - convertTicksPer100msToRPM(shooterWheelRight.getSelectedSensorVelocity(0))); + SmartDashboard.putNumber("Shooter Wheel R-Voltage", shooterWheelRight.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter Wheel R-Bus Voltage", shooterWheelRight.getBusVoltage()); + SmartDashboard.putNumber("Shooter Wheel R-Current", shooterWheelRight.getSupplyCurrent()); + } + + public void zero() { + } + + /** + * Set shooter to rpm speed. + * + * @param rpm + */ + public void setSpeed(double rpm) { + + if (rpm > MAX_SPEED_RPM) { + rpm = MAX_SPEED_RPM; + } + + m_targetSpeedRPM = rpm; + double ticks = convertRpmToTicksPer100ms(rpm); + shooterWheelLeft.set(ControlMode.Velocity, ticks); + shooterWheelRight.set(ControlMode.Velocity, ticks); + } + + public void setSpeedVBus(double pos) { + shooterWheelLeft.set(ControlMode.PercentOutput, pos); + shooterWheelRight.set(ControlMode.PercentOutput, pos); + } + + public double getSpeed() { + return convertTicksPer100msToRPM(shooterWheelLeft.getSelectedSensorVelocity(0)); + } + + public double getTargetSpeed() { + return m_targetSpeedRPM; + } + + public double getSpeedVBus() { + return shooterWheelLeft.getMotorOutputVoltage(); + } + + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return shooterWheelLeft.getP(); + } + + @Override + public double getI() { + return shooterWheelLeft.getI(); + } + + @Override + public double getD() { + return shooterWheelLeft.getD(); + } + + @Override + public double getF() { + return shooterWheelLeft.getF(); + + } + + @Override + public void setP(double d) { + shooterWheelLeft.config_kP(0, d, 0); + shooterWheelRight.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + shooterWheelLeft.config_kI(0, d, 0); + shooterWheelRight.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + shooterWheelLeft.config_kD(0, d, 0); + shooterWheelRight.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + shooterWheelLeft.config_kF(0, d, 0); + shooterWheelRight.config_kF(0, d, 0); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java new file mode 100644 index 0000000..93e6a9e --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Targeting.java @@ -0,0 +1,286 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.RobotContainer; + +// import org.mayheminc.robot2019.Robot; + +// import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +// import sun.net.www.content.text.plain; + +/** + * Add your docs here. + */ + +public class Targeting extends SubsystemBase { + + // Logitech C920 Field of View Information: + // Diagonal FOV = 78.0 + // Horizontal FOV = 70.42 + // Vertical FOV = 43.3 + // NOTE: 76.5 horizontal FOV determined empirically by Ken on 2/22/2020 + // private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 76.5; + + // oCam 1CGN Field of View Information: + // FOV = 65.0 + // NOTE: 86.5 horizontal FOV determined empirically by Ken, Amy, and Caleb on + // 9/24/2020 + private final static double FOV_HORIZ_CAMERA_IN_DEGREES = 63.0; // was 86.5 on 4 Nov 2020 + + // Define the "target location" to be halfway from left to right + private final double CENTER_OF_TARGET_X = 0.475; + + private final double BEST_Y_CLOSE_THRESHOLD = 0.1; + private final double CLOSE_WHEEL_SPEED = 3000.0; + private final double CLOSE_HOOD_ANGLE = 30000.0; + + // After computing a desired azimuth, add a "fudge" offset to correct + // empirically measured error. Offset should be in azimuth "ticks." + private static final double AZIMUTH_CORRECTION_OFFSET = 0.0; // was -2.0 at CMP + + // TODO, inner port depth in feet + private static final double INNER_PORT_DEPTH = 29.5 / 12.0; + + private double m_desiredAzimuth; + private double m_desiredHood; + private double m_desiredWheelSpeed; + private double[] m_target_array; + private int m_priorFrameCount; + private double m_priorFrameTime; + private double[] ARRAY_OF_NEG_ONE = { -1.0 }; + + private double m_bestY = 0.0; + private double m_bestX = 0.0; + private double m_tilt = 0.0; + private double m_area = 0.0; + + public enum TargetPosition { + LEFT_MOST, CENTER_MOST, RIGHT_MOST, CENTER_OF_RIGHT_CARGO_SHIP, CENTER_OF_LEFT_CARGO_SHIP + }; + + public enum TargetHeight { + CARGO, HATCH + }; + + @Override + public void periodic() { + update(); + } + + // TODO: make an updateSmartDashboard() method in Targeting for optimization + public void update() { + // perform periodic update functions for the targeting capability + int latestFrameCount = (int) SmartDashboard.getNumber("frameCount", -1.0 /* default to -1 */); + if (latestFrameCount < 0) { + // an invalid number for latestFrameCount, display warning light + SmartDashboard.putBoolean("visionOK", false); + SmartDashboard.putString("visionOkDebug", "No Data"); + } else if (latestFrameCount == m_priorFrameCount) { + // have not received a new frame. If more than 1 second has elapsed since + // prior new frame, display a warning light on the SmartDashboard + if (Timer.getFPGATimestamp() > m_priorFrameTime + 1.0) { + SmartDashboard.putBoolean("visionOK", false); + SmartDashboard.putString("visionOkDebug", "Stale Data"); + } else { + // else, have an old frame, but it's not too old yet, so do nothing + } + } else { + // have received a new frame, save the time and update m_priorFrameCount + m_priorFrameTime = Timer.getFPGATimestamp(); + m_priorFrameCount = latestFrameCount; + SmartDashboard.putBoolean("visionOK", true); + SmartDashboard.putString("visionOkDebug", "Good Data"); + } + + // Update all of the targeting information, as follows: + // 1 - Determine if we have any valid data in the array. + // If not, set the "error" to zero, so that the robot thinks + // it is on target. + // 2 - Use the target to compute the needed information + + // get the latest output from the targeting camera + m_target_array = SmartDashboard.getNumberArray("target", ARRAY_OF_NEG_ONE); + + if (m_target_array == null || m_target_array.length == 0 || m_target_array.length == 1) { + // this means the key is found, but is empty + m_bestX = 0.0; + m_bestY = 0.0; + m_tilt = 0.0; + m_area = 0.0; + // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); + // } else if (m_target_array[0] < 0.0) { + // // this means the array has no valid data. Set m_xError = 0.0 + // m_bestX = 0.0; + // m_bestY = 0.0; + // m_tilt = 0.0; + // m_area = 0.0; + // // m_desiredAzimuth = RobotContainer.turret.getAzimuthForCapturedImage(); + } else { + // We have a valid data array. + // There are three different situations: + // a - We want the left-most target + // b - We want the "centered" target + // c - We want the right-most target + + // Handle each of them separately; + // we need the results in "bestXError" and "bestY" + // NOTEL m_target_array[0] is now the frame count, but we're not using it yet + m_bestX = m_target_array[1]; // get the x-value + m_bestY = m_target_array[2]; // get the y-value + m_tilt = m_target_array[3]; + m_area = m_target_array[4]; + + m_desiredAzimuth = findDesiredAzimuthOuterPort(m_bestX, m_bestY, m_tilt, m_area); + + // TODO: The following code calculates the inner port angle! + // + // double outerPortTicks = findDesiredAzimuthOuterPort(m_bestX, m_bestY, m_tilt, + // m_area); + // double outerPortDegrees = outerPortTicks / Turret.TICKS_PER_DEGREE; + + // double innerPortDegrees = getAngleToInnerPort(outerPortDegrees); + // m_desiredAzimuth = innerPortDegrees * Turret.TICKS_PER_DEGREE; + + m_desiredHood = getHoodFromY(); + m_desiredWheelSpeed = getWheelSpeedFromY(); + } + + // at this point in the code, the "selected" target should be in the "best" + SmartDashboard.putNumber("m_bestX", m_bestX); + SmartDashboard.putNumber("m_bestY", m_bestY); + SmartDashboard.putNumber("m_tilt", m_tilt); + SmartDashboard.putNumber("m_area", m_area); + + SmartDashboard.putNumber("Range per Y", this.getRangeFromY()); + SmartDashboard.putNumber("Range per Area", this.getRangeFromArea()); + + SmartDashboard.putNumber("Wheel Speed From Y", this.getWheelSpeedFromY()); + SmartDashboard.putNumber("Hood Angle From Y", this.getHoodFromY()); + } + + public double getDesiredAzimuth() { + return m_desiredAzimuth + AZIMUTH_CORRECTION_OFFSET; + } + + public double getDesiredHood() { + return m_desiredHood; + } + + public double getDesiredWheelSpeed() { + return m_desiredWheelSpeed; + } + + /** + * Return the desired turrent encoder ticks in the turret for the center of the + * target. + * + * @param X + * @param Y + * @param tilt + * @param area + * @return + */ + public double findDesiredAzimuthOuterPort(double X, double Y, double tilt, double area) { + // Calulate angle error based on an X,Y + double angleError; + double ticksError; + double xError; + double desiredAzimuth; + + // compute the "x error" based upon the center for shooting + xError = X - CENTER_OF_TARGET_X; + // Find the angle error + angleError = FOV_HORIZ_CAMERA_IN_DEGREES * xError; + // convert the angle error into ticks + ticksError = angleError * Turret.TICKS_PER_DEGREE; + + // Convert angleError into a desired azimuth, using the azimuth history + desiredAzimuth = ticksError + RobotContainer.turret.getAzimuthForCapturedImage(); + // Update SmartDashboard + SmartDashboard.putNumber("Vision Angle Error", angleError); + SmartDashboard.putNumber("Vision Desired Azimuth", desiredAzimuth + Math.random()); + return desiredAzimuth; + } + + /** + * \(owo)/ <- hi! Computes the angle to the inner port! + * + * @return + */ + public double getAngleToInnerPort(double angleToOuterPort) { + double range = getRangeFromArea(); + double degreesToRadians = 2 * Math.PI / 360; + + double x = range * Math.sin(angleToOuterPort * degreesToRadians); + double y = range * Math.cos(angleToOuterPort * degreesToRadians); + + return Math.atan(x / (y + INNER_PORT_DEPTH)) / degreesToRadians; + } + + /** + * use m_bestY to get the desired hood setting for the target + * + * @return + */ + private double getHoodFromY() { + // below is the "curve fit" for the "long shot" + + if (m_bestY < BEST_Y_CLOSE_THRESHOLD) { + // too close for the lob shot, switch to the bullet shot + return CLOSE_HOOD_ANGLE; + } else { + // y(x) = -33,064x^2 + 37,144x^1 + -5,872 + return -33064 * m_bestY * m_bestY + 37144*m_bestY - 5872; + } + } + + /** + * use m_bestY to get the desired wheel speed for the target + * + * @return + */ + private double getWheelSpeedFromY() { + double computedWheelSpeed = -137 + 14201 * m_bestY + -10089 * m_bestY * m_bestY; + // -10,089x^2 + 14,201x^1 + -137 + + if (m_bestY < BEST_Y_CLOSE_THRESHOLD) { + // too close for the lob shot, switch to the bullet shot + computedWheelSpeed = CLOSE_WHEEL_SPEED; + } + + if (computedWheelSpeed < ShooterWheel.IDLE_SPEED) { + computedWheelSpeed = ShooterWheel.IDLE_SPEED; + } else if (computedWheelSpeed > ShooterWheel.MAX_SPEED_RPM) { + computedWheelSpeed = ShooterWheel.MAX_SPEED_RPM; + } + return computedWheelSpeed; + } + + /** + * use m_bestY to get the range in feet to the target. + * + * @return + */ + private double getRangeFromY() { + return 8.11 + -9.17 * m_bestY + 33.9 * m_bestY * m_bestY; + } + + /** + * Use the area to calculate the range in feet. + * + * @return + */ + private double getRangeFromArea() { + return 0.912 * Math.pow(m_area, -0.695); + } + +} diff --git a/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java new file mode 100644 index 0000000..0f5f3ec --- /dev/null +++ b/src/main/java/org/mayheminc/robot2020/subsystems/Turret.java @@ -0,0 +1,237 @@ +package org.mayheminc.robot2020.subsystems; + +import org.mayheminc.robot2020.Constants; +import org.mayheminc.robot2020.RobotContainer; + +import com.ctre.phoenix.motorcontrol.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.mayheminc.util.History; +import org.mayheminc.util.MayhemTalonSRX; +import org.mayheminc.util.PidTunerObject; +import org.mayheminc.util.MayhemTalonSRX.CurrentLimit; + +public class Turret extends SubsystemBase implements PidTunerObject { + private final MayhemTalonSRX turretTalon = new MayhemTalonSRX(Constants.Talon.SHOOTER_TURRET, + CurrentLimit.LOW_CURRENT); + + public static final boolean WAIT_FOR_DONE = true; + private final int MIN_POSITION = -38000; // approx -270 degrees + private final int MAX_POSITION = +3000; // approx +25 degrees + private final int DESTINATION_TOLERANCE = 200; + + // if "at destination" want the "I" to get us as close as possible + private final int INTEGRAL_ZONE = 100; + + // TICKS_PER_DEGREE computed by 4096 ticks per rotation of shaft / 1 rotation of + // shaft per 18t * 225t / 1 rotation of turret + + public final static double TICKS_PER_DEGREE = 4096.0 / 18.0 * 225.0 / 360.0; + // above works out to 142.2 ticks per degree + // a full circle is 51,200 ticks + + double m_targetSpeedRPM = 0.0; + double m_desiredPosition = 0.0; + History headingHistory = new History(); + + /** + * Creates a new Turret. + */ + public Turret() { + configureTurretTalon(); + } + + public void init() { + configureTurretTalon(); + } + + void configureTurretTalon() { + // PID tuning notes: + // during competition season, used P=1.0; everything else zero + // on 3 December tuned with Caleb, Amy, Coach Streeter with P=0.7, D=7.0 + // Note: had "overshoot" issues when using I. (Tried 0.001 to 0.01) + turretTalon.config_kP(0, 0.7, 0); + turretTalon.config_kI(0, 0.01, 0); + turretTalon.config_kD(0, 7.0, 0); + turretTalon.config_kF(0, 0.0, 0); + + // experimented by adding "integral zone" for when turret is close to intended + // target + turretTalon.config_IntegralZone(0, INTEGRAL_ZONE); + turretTalon.changeControlMode(ControlMode.Position); + turretTalon.setNeutralMode(NeutralMode.Coast); + turretTalon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder); + + turretTalon.configMotionCruiseVelocity(2500); // max velocity at 100% is about 3200, cruise at 80% + turretTalon.configMotionAcceleration(5000); // acceleration of 2x velocity, allows cruise in 1/2 second + + turretTalon.configNominalOutputVoltage(+0.0f, -0.0f); + turretTalon.configPeakOutputVoltage(+12.0, -12.0); + + turretTalon.configForwardSoftLimitThreshold(MAX_POSITION); + turretTalon.configForwardSoftLimitEnable(true); + turretTalon.configReverseSoftLimitThreshold(MIN_POSITION); + turretTalon.configReverseSoftLimitEnable(true); + + turretTalon.setInverted(false); + turretTalon.setSensorPhase(false); + + this.setVBus(0.0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + UpdateDashboard(); + updateHistory(); + } + + // KBS: tuned below at practice field on 21 Feb 2020 via successive refinement + // to get 0.08 w/Logitech C920 camera. + + // KBS: tuned below at practice field on 24 Sept 2020 w/Caleb and Amy to be 0.17 + // w/1CGN camera + private static final double CAMERA_LAG = 0.17; // .05 was best so far in 2020; used .150 in 2019 + + private void updateHistory() { + double now = Timer.getFPGATimestamp(); + headingHistory.add(now, getPosition()); + } + + public double getAzimuthForCapturedImage() { + double now = Timer.getFPGATimestamp(); + double indexTime = now - CAMERA_LAG; + return headingHistory.getAzForTime(indexTime); + } + + private void UpdateDashboard() { + SmartDashboard.putNumber("Shooter turret pos", turretTalon.getPosition()); + SmartDashboard.putNumber("Shooter turret pos desired", m_desiredPosition); + SmartDashboard.putNumber("Shooter turret vbus", turretTalon.getMotorOutputVoltage()); + SmartDashboard.putNumber("Shooter turret velocity", turretTalon.getSelectedSensorVelocity(0)); + } + + public void zero() { + m_desiredPosition = 0.0; + turretTalon.setPosition(0); + } + + /** + * Set the absolute turret position. + */ + public void setPositionAbs(double pos) { + if (pos < MIN_POSITION) { + pos = MIN_POSITION; + } + if (pos > MAX_POSITION) { + pos = MAX_POSITION; + } + m_desiredPosition = pos; + turretTalon.set(ControlMode.MotionMagic, pos); + } + + /** + * Set the relative turret position + * + * @param pos number of encoder ticks to adjust. + */ + public void setPositionRel(double pos) { + // Below line sets position relative to most recent "desiredPosition" but may + // have BAD side-effect if turret had just been in VBus mode (i.e. may not have + // been near prior desiredPosition) Would need to decide a safer way to know + // whether to have new relative position be relative to the current + // desiredPosition or the current actual position. + + // m_desiredPosition = m_desiredPosition + pos; + m_desiredPosition = getPosition() + pos; + setPositionAbs(m_desiredPosition); + } + + public void setVBus(double power) { + turretTalon.set(ControlMode.PercentOutput, power); + } + + /** + * Get the current position of the turret (in encoder counts). + * + * @return + */ + public double getPosition() { + return turretTalon.getPosition(); + } + + /** + * Get the desired position of the turret. + * + * @return + */ + public double getDesiredPosition() { + return m_desiredPosition; + } + + /** + * Gets the turret's absolute position, in degrees, relative to the robot's + * heading. + * + * @return + */ + public double getGlobalTurretPosition() { + return RobotContainer.drive.getHeading() + (this.getPosition() / TICKS_PER_DEGREE); + } + + /** + * Return true if close enough to desired position + * + * @return + */ + public boolean isAtDesiredPosition() { + return (Math.abs(getPosition() - getDesiredPosition()) < DESTINATION_TOLERANCE); + } + + //////////////////////////////////////////////////// + // PidTunerObject + @Override + public double getP() { + return turretTalon.getP(); + } + + @Override + public double getI() { + return turretTalon.getI(); + } + + @Override + public double getD() { + return turretTalon.getD(); + } + + @Override + public double getF() { + return turretTalon.getF(); + + } + + @Override + public void setP(double d) { + turretTalon.config_kP(0, d, 0); + } + + @Override + public void setI(double d) { + turretTalon.config_kI(0, d, 0); + } + + @Override + public void setD(double d) { + turretTalon.config_kD(0, d, 0); + } + + @Override + public void setF(double d) { + turretTalon.config_kF(0, d, 0); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/AndJoystickButton.java b/src/main/java/org/mayheminc/util/AndJoystickButton.java new file mode 100644 index 0000000..2c4da30 --- /dev/null +++ b/src/main/java/org/mayheminc/util/AndJoystickButton.java @@ -0,0 +1,23 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.*; + +public class AndJoystickButton extends Button { + GenericHID joystick1; + GenericHID joystick2; + int buttonNumber1; + int buttonNumber2; + + public AndJoystickButton(GenericHID joystick1, int buttonNumber1, GenericHID joystick2, int buttonNumber2) { + this.joystick1 = joystick1; + this.buttonNumber1 = buttonNumber1; + this.joystick2 = joystick2; + this.buttonNumber2 = buttonNumber2; + } + + public boolean get() { + return joystick1.getRawButton(buttonNumber1) && joystick2.getRawButton(buttonNumber2); + } + +} diff --git a/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java b/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java new file mode 100644 index 0000000..a76ab1c --- /dev/null +++ b/src/main/java/org/mayheminc/util/DisabledOnlyJoystickButton.java @@ -0,0 +1,26 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.*; + +/** + * + * @author Team1519 + */ +public class DisabledOnlyJoystickButton extends Button { + + private GenericHID joystick; + private int buttonNumber; + private DriverStation ds; + + public DisabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { + this.joystick = joystick; + this.buttonNumber = buttonNumber; + ds = DriverStation.getInstance(); + } + + public boolean get() { + return joystick.getRawButton(buttonNumber) && ds.isDisabled(); + } +} diff --git a/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java b/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java new file mode 100644 index 0000000..9bd9d4a --- /dev/null +++ b/src/main/java/org/mayheminc/util/EnabledOnlyJoystickButton.java @@ -0,0 +1,26 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.button.*; + +/** + * + * @author Team1519 + */ +public class EnabledOnlyJoystickButton extends Button { + + private GenericHID joystick; + private int buttonNumber; + private DriverStation ds; + + public EnabledOnlyJoystickButton(GenericHID joystick, int buttonNumber) { + this.joystick = joystick; + this.buttonNumber = buttonNumber; + ds = DriverStation.getInstance(); + } + + public boolean get() { + return joystick.getRawButton(buttonNumber) && ds.isEnabled(); + } +} diff --git a/src/main/java/org/mayheminc/util/EventServer/Event.java b/src/main/java/org/mayheminc/util/EventServer/Event.java new file mode 100644 index 0000000..c968a2d --- /dev/null +++ b/src/main/java/org/mayheminc/util/EventServer/Event.java @@ -0,0 +1,6 @@ +package org.mayheminc.util.EventServer; + +public abstract class Event { + public abstract String Execute(); + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/EventServer/EventServer.java b/src/main/java/org/mayheminc/util/EventServer/EventServer.java new file mode 100644 index 0000000..cc77195 --- /dev/null +++ b/src/main/java/org/mayheminc/util/EventServer/EventServer.java @@ -0,0 +1,52 @@ +package org.mayheminc.util.EventServer; + +import java.util.Vector; + +/** + * The Event Server has a TCP Server. Every 100 ms it loops through the events + * and lets them send messages to the TCP Server. + */ +public class EventServer extends Thread { + + Vector EventList = new Vector(); + TCPServer tcpServer = new TCPServer(); + + /** + * add an event object to the event list that will be polled every 100ms. + * + * @param E + */ + public void add(Event E) { + EventList.add(E); + } + + /** + * output a string to the TCP Server. This can be used by the events to signal + * thier events or single events can fire strings directly to the TCP Server + * + * @param str + */ + public void output(String str) { + tcpServer.add(str); + } + + /** + * Run the event server. Run the TCP Server. Loop through the events every + * 100ms. + */ + public void run() { + tcpServer.start(); + + while (true) { + try { + Thread.sleep(100); + + // execute each event + EventList.forEach((n) -> n.Execute()); + + } catch (InterruptedException e) { + e.printStackTrace(); + } + } + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java b/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java new file mode 100644 index 0000000..f8b7e9c --- /dev/null +++ b/src/main/java/org/mayheminc/util/EventServer/OneTimeEvent.java @@ -0,0 +1,18 @@ +package org.mayheminc.util.EventServer; + +public abstract class OneTimeEvent extends Event { + boolean hasExecuted = false; + + public String Execute() { + if (hasExecuted) { + return ""; + } + String S = this.OneTimeExecute(); + if (S != "") { + hasExecuted = true; + } + return S; + } + + abstract public String OneTimeExecute(); +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/EventServer/TCPServer.java b/src/main/java/org/mayheminc/util/EventServer/TCPServer.java new file mode 100644 index 0000000..b44ec9a --- /dev/null +++ b/src/main/java/org/mayheminc/util/EventServer/TCPServer.java @@ -0,0 +1,65 @@ +package org.mayheminc.util.EventServer; + +import java.io.*; +import java.net.*; +import java.util.concurrent.ArrayBlockingQueue; + +import edu.wpi.first.wpilibj.DriverStation; + +class TCPServer extends Thread { + + private static final int PORT = 5809; + + ArrayBlockingQueue buffer = new ArrayBlockingQueue<>(10); + + public static void main(String argv[]) throws Exception { + TCPServer server = new TCPServer(); + server.start(); + + Thread.sleep(5000); + server.add("bugs_answer.wav\n"); + + Thread.sleep(5000); + server.add("bugs_answer.wav\n"); + Thread.sleep(5000); + server.add("bugs_answer.wav\n"); + Thread.sleep(5000); + server.add("bugs_answer.wav\n"); + } + + public void add(String S) { + // add S to the buffer, but if we can't fail silently + buffer.offer(S); + } + + public void run() { + ServerSocket welcomeSocket; + try { + String wavfile; + + welcomeSocket = new ServerSocket(PORT); + + try { + while (true) { + Socket connectionSocket = welcomeSocket.accept(); + + System.out.println("Opening socket"); + DriverStation.reportError("Opening Socket for Sound", false); + + DataOutputStream outToClient = new DataOutputStream(connectionSocket.getOutputStream()); + + while (true) { + wavfile = buffer.take(); // take from the buffer. Wait if nothing is available + outToClient.writeBytes(wavfile + "\n"); + DriverStation.reportError("Sending Sound: " + wavfile, false); + } + } + } catch (Exception ex) { + } finally { + welcomeSocket.close(); + } + + } catch (Exception ex) { + } + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java b/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java new file mode 100644 index 0000000..22d07f2 --- /dev/null +++ b/src/main/java/org/mayheminc/util/EventServer/TMinusEvent.java @@ -0,0 +1,24 @@ +package org.mayheminc.util.EventServer; + +import edu.wpi.first.wpilibj.DriverStation; + +public class TMinusEvent extends OneTimeEvent { + String name; + double time; + final double MATCH_TIME_SEC = 150.0; + + public TMinusEvent(String S, int T) { + name = S; + time = T; + } + + public String OneTimeExecute() { + + // if the match time left is less than the time, return the name. + if (DriverStation.getInstance().getMatchTime() < time) { + return name; + } + return ""; + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/History.java b/src/main/java/org/mayheminc/util/History.java new file mode 100644 index 0000000..b91707f --- /dev/null +++ b/src/main/java/org/mayheminc/util/History.java @@ -0,0 +1,59 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.DriverStation; + +public class History { + private static final int HISTORY_SIZE = 50; + + private double time[] = new double[HISTORY_SIZE]; + private double azimuth[] = new double[HISTORY_SIZE]; + private int index = 0; + + public History() { + // ensure there is at least one element in the history + add(-1.0, 0.0); // make a fictitious element at t=-1 seconds, with heading of 0.0 degrees + } + + public void add(double t, double az) { + + time[index] = t; + azimuth[index] = az; + + index++; + if (index >= HISTORY_SIZE) { + index = 0; + } + } + + public double getAzForTime(double t) { + double az = azimuth[index]; + int i = index - 1; + int count = 0; + + // if (t < 0) { + // DriverStation.reportError("Negative time in history", false); + // return 0.0; // no negative times. + // } + + while (i != index) { + if (i < 0) { + i = HISTORY_SIZE - 1; + } + + if (time[i] <= t) { + az = azimuth[i]; + break; + } + + i--; + count++; + if (count > HISTORY_SIZE) { + DriverStation.reportError("Looking too far back", false); + az = azimuth[index]; + break; + } + } + + return az; + } +} diff --git a/src/main/java/org/mayheminc/util/JoystickAxisButton.java b/src/main/java/org/mayheminc/util/JoystickAxisButton.java new file mode 100644 index 0000000..0657aa4 --- /dev/null +++ b/src/main/java/org/mayheminc/util/JoystickAxisButton.java @@ -0,0 +1,91 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.*; + +/** + * + * @author Team1519 + */ +public class JoystickAxisButton extends Button { + public static final int BOTH_WAYS = 1; + public static final int POSITIVE_ONLY = 2; + public static final int NEGATIVE_ONLY = 3; + + private static final double AXIS_THRESHOLD = 0.2; + + private Joystick joystick; + private Joystick.AxisType axis; + private int axisInt; + private int direction; + + private double getAxis(Joystick.AxisType axis) { + switch (axis) { + case kX: + return joystick.getX(); + case kY: + return joystick.getY(); + case kZ: + return joystick.getZ(); + case kThrottle: + return joystick.getThrottle(); + case kTwist: + return joystick.getTwist(); + default: + // Unreachable + return 0.0; + } + } + + public JoystickAxisButton(Joystick stick, Joystick.AxisType axis) { + this(stick, axis, BOTH_WAYS); + } + + public JoystickAxisButton(Joystick stick, Joystick.AxisType axis, int direction) { + joystick = stick; + this.axis = axis; + this.direction = direction; + } + + public JoystickAxisButton(Joystick stick, int axis) { + this(stick, axis, BOTH_WAYS); + } + + public JoystickAxisButton(Joystick stick, int axis, int direction) { + joystick = stick; + this.axis = null; + axisInt = axis; + this.direction = direction; + } + + public boolean get() { + switch (direction) { + case BOTH_WAYS: + if (axis != null) { + return Math.abs(getAxis(axis)) > AXIS_THRESHOLD; + } else { + return Math.abs(joystick.getRawAxis(axisInt)) > AXIS_THRESHOLD; + } + + case POSITIVE_ONLY: + if (axis != null) { + return getAxis(axis) > AXIS_THRESHOLD; + } else { + return joystick.getRawAxis(axisInt) > AXIS_THRESHOLD; + } + + case NEGATIVE_ONLY: + if (axis != null) { + return getAxis(axis) < -AXIS_THRESHOLD; + } else { + return joystick.getRawAxis(axisInt) < -AXIS_THRESHOLD; + } + } + + return false; + } +} diff --git a/src/main/java/org/mayheminc/util/JoystickPOVButton.java b/src/main/java/org/mayheminc/util/JoystickPOVButton.java new file mode 100644 index 0000000..a061fc4 --- /dev/null +++ b/src/main/java/org/mayheminc/util/JoystickPOVButton.java @@ -0,0 +1,34 @@ +/* + * To change this template, choose Tools | Templates + * and open the template in the editor. + */ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.*; + +/** + * @author Team1519 + */ +public class JoystickPOVButton extends Button { + public static final int NORTH = 0; + public static final int NORTHEAST = 45; + public static final int EAST = 90; + public static final int SOUTHEAST = 135; + public static final int SOUTH = 180; + public static final int SOUTHWEST = 225; + public static final int WEST = 270; + public static final int NORTHWEST = 315; + + private Joystick joystick; + private int desiredPOV; + + public JoystickPOVButton(Joystick stick, int newDesiredPOV) { + joystick = stick; + desiredPOV = newDesiredPOV; + } + + public boolean get() { + return (joystick.getPOV() == desiredPOV); + } +} diff --git a/src/main/java/org/mayheminc/util/Joysticks.java b/src/main/java/org/mayheminc/util/Joysticks.java new file mode 100644 index 0000000..73133c5 --- /dev/null +++ b/src/main/java/org/mayheminc/util/Joysticks.java @@ -0,0 +1,8 @@ +package org.mayheminc.util; + +public final class Joysticks { + public static final int DRIVER_GAMEPAD = 0; + public static final int DRIVER_JOYSTICK = 1; + public static final int OPERATOR_GAMEPAD = 2; + public static final int OPERATOR_JOYSTICK = 3; +} diff --git a/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java b/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java new file mode 100644 index 0000000..76442a5 --- /dev/null +++ b/src/main/java/org/mayheminc/util/MB1340Ultrasonic.java @@ -0,0 +1,31 @@ +package org.mayheminc.util; +import edu.wpi.first.wpilibj.AnalogInput; + +/* + * The Ultrasonic sensor being used is an MB1340. + * The conversion factor being used is (5 volts) / (1024 per cm). + */ + +public class MB1340Ultrasonic { + AnalogInput analogUltrasonic; + final static double voltage = 5.0; + final static double maxADCBins = 1024; + + public MB1340Ultrasonic(int analogPort) { + analogUltrasonic = new AnalogInput(analogPort); + } + + /** + * Get the Distance in cm. + * Full scale is 1024 cm. The minimum is 25cm. + * @return + */ + public double getDistance() { + return (analogUltrasonic.getAverageVoltage() / voltage) * maxADCBins; + } + + public double getDistanceInInches() { + return getDistance() / 2.54; + } + +} diff --git a/src/main/java/org/mayheminc/util/MayhemDriverPad.java b/src/main/java/org/mayheminc/util/MayhemDriverPad.java new file mode 100644 index 0000000..b4c4e5e --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemDriverPad.java @@ -0,0 +1,156 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +public class MayhemDriverPad { + + public final class GAMEPAD_BUTTION { + public static final int GAMEPAD_F310_A_BUTTON = 1; + public static final int GAMEPAD_F310_B_BUTTON = 2; + public static final int GAMEPAD_F310_X_BUTTON = 3; + public static final int GAMEPAD_F310_Y_BUTTON = 4; + public static final int GAMEPAD_F310_LEFT_BUTTON = 5; + public static final int GAMEPAD_F310_RIGHT_BUTTON = 6; + public static final int GAMEPAD_F310_BACK_BUTTON = 7; + public static final int GAMEPAD_F310_START_BUTTON = 8; + public static final int GAMEPAD_F310_LEFT_STICK_BUTTON = 9; + public static final int GAMEPAD_F310_RIGHT_STICK_BUTTON = 10; + } + + public final class GAMEPAD_AXIS { + public static final int GAMEPAD_F310_LEFT_X_AXIS = 0; + public static final int GAMEPAD_F310_LEFT_Y_AXIS = 1; + public static final int GAMEPAD_F310_LEFT_TRIGGER = 2; + public static final int GAMEPAD_F310_RIGHT_TRIGGER = 3; + public static final int GAMEPAD_F310_RIGHT_X_AXIS = 4; + public static final int GAMEPAD_F310_RIGHT_Y_AXIS = 5; + } + + public final Joystick DRIVER_PAD = new Joystick(Joysticks.DRIVER_GAMEPAD);; + + public final Button DRIVER_PAD_BUTTON_FIVE = new JoystickButton(DRIVER_PAD, 5); // Left Top Trigger; + public final Button DRIVER_PAD_BUTTON_SIX = new JoystickButton(DRIVER_PAD, 6); // Right Top Trigger; + public final Button DRIVER_PAD_LEFT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_STICK_BUTTON); // Left; + public final Button DRIVER_PAD_RIGHT_STICK_BUTTON = new JoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_STICK_BUTTON); // Right; + + public final JoystickPOVButton DRIVER_PAD_D_PAD_UP = new JoystickPOVButton(DRIVER_PAD, 0); + public final JoystickPOVButton DRIVER_PAD_D_PAD_RIGHT = new JoystickPOVButton(DRIVER_PAD, 90); + public final JoystickPOVButton DRIVER_PAD_D_PAD_DOWN = new JoystickPOVButton(DRIVER_PAD, 180); + public final JoystickPOVButton DRIVER_PAD_D_PAD_LEFT = new JoystickPOVButton(DRIVER_PAD, 270); + public final Button CLIMB_L3_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_Y_BUTTON, DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); // Y=Yellow, + // plus + // left + // top + // trigger + public final Button CLIMB_L2_BUTTON_PAIR = new AndJoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_A_BUTTON, DRIVER_PAD, GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON);// A=Green, + // plus + // left + // top + // trigger + + // Driver Control Modes + public final Button TOGGLE_CLOSED_LOOP_MODE_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 7); + public final Button TOGGLE_FOD_BUTTON = new DisabledOnlyJoystickButton(DRIVER_PAD, 8); + + // NOTE: DRIVER_PAD_RIGHT_UPPER_TRIGGER_BUTTON is "QUICKTURN" in Drive.java - DO + // NOT USE HERE!!! + public final Button DRIVER_PAD_LEFT_UPPER_TRIGGER_BUTTON = new EnabledOnlyJoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_LEFT_BUTTON); + public final JoystickAxisButton DRIVER_PAD_LEFT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + GAMEPAD_AXIS.GAMEPAD_F310_LEFT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + public final JoystickAxisButton DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON = new JoystickAxisButton(DRIVER_PAD, + GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_TRIGGER, JoystickAxisButton.POSITIVE_ONLY); + + public final Button DRIVER_PAD_BACK_BUTTON = new JoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_BACK_BUTTON); + public final Button DRIVER_PAD_START_BUTTON = new JoystickButton(DRIVER_PAD, + GAMEPAD_BUTTION.GAMEPAD_F310_START_BUTTON); + + public final Button DRIVER_PAD_GREEN_BUTTON = new JoystickButton(DRIVER_PAD, 1);; + public final Button DRIVER_PAD_RED_BUTTON = new JoystickButton(DRIVER_PAD, 2); // RED 'B" button + public final Button DRIVER_PAD_BLUE_BUTTON = new JoystickButton(DRIVER_PAD, 3); // BLUE 'X' button + public final Button DRIVER_PAD_YELLOW_BUTTON = new JoystickButton(DRIVER_PAD, 4); // YELLOW 'Y' button + + public MayhemDriverPad() { + } + + private static final double THROTTLE_DEAD_ZONE_PERCENT = 0.05; + private static final double MIN_THROTTLE_FOR_MOVEMENT = 0.02; // what is the min throttle for Movement + private static final double NORMAL_MAX_THROTTLE = 1.00; // maximum speed is normally 100% + private static final double SLOW_MODE_MAX_THROTTLE = 0.30; // maximum throttle in "slow mode" is 30% + + public double driveThrottle() { + // the driveThrottle is the "Y" axis of the Driver Gamepad. + // However, the joysticks give -1.0 on the Y axis when pushed forward + // This method reverses that, so that positive numbers are forward + double throttleVal = -DRIVER_PAD.getY(); + double throttleAbs = Math.abs(throttleVal); + double maxPercentThrottle = NORMAL_MAX_THROTTLE; + + if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { + // check for "slow mode" and if so, constrain maxPercentThrottle to "SLOW_MODE_MAX_PERCENT" + maxPercentThrottle = SLOW_MODE_MAX_THROTTLE; + } + + // compute a scaled throttle magnitude, which will always be positive + double throttleMag = MIN_THROTTLE_FOR_MOVEMENT + (maxPercentThrottle - MIN_THROTTLE_FOR_MOVEMENT) * throttleAbs * throttleAbs; + + if (Math.abs(throttleVal) < THROTTLE_DEAD_ZONE_PERCENT) { + // check for throttle being in "dead zone" and if so, set throttle to zero + throttleVal = 0.0; + } else { + // make sure to preserve sign of throttle + if (throttleVal >= 0.0) { + throttleVal = throttleMag; + } else { + throttleVal = -throttleMag; + } + } + + return (throttleVal); + } + + private static final double STEERING_DEAD_ZONE_PERCENT = 0.05; + private static final double MIN_STEERING_FOR_MOVEMENT = 0.02; + private static final double NORMAL_MAX_STEERING = 1.00; + private static final double SLOW_MODE_MAX_STEERING = 0.80; // maximum steering in "slow mode" is 50% + + public double steeringX() { + // SteeringX is the "X" axis of the right stick on the Driver Gamepad. + double steeringVal = DRIVER_PAD.getRawAxis(GAMEPAD_AXIS.GAMEPAD_F310_RIGHT_X_AXIS); + double steeringAbs = Math.abs(steeringVal); + double maxPercentSteering = NORMAL_MAX_STEERING; + + if (DRIVER_PAD_RIGHT_LOWER_TRIGGER_BUTTON.get()) { + // check for "slow mode" and if so, constrain maxPercentSteering to "SLOW_MODE_MAX_PERCENT" + maxPercentSteering = SLOW_MODE_MAX_STEERING; + } + + // compute a scaled steering magnitude, which will always be positive + double steeringMag = MIN_STEERING_FOR_MOVEMENT + (maxPercentSteering - MIN_STEERING_FOR_MOVEMENT) * steeringAbs; + + if (Math.abs(steeringVal) < STEERING_DEAD_ZONE_PERCENT) { + // check for steering being in "dead zone" and if so, set steering to zero + steeringVal = 0.0; + } else { + // make sure to preserve sign of steering + if (steeringVal >= 0.0) { + steeringVal = steeringMag; + } else { + steeringVal = -steeringMag; + } + } + + return (steeringVal); + } + + public boolean quickTurn() { + return (DRIVER_PAD.getRawButton(GAMEPAD_BUTTION.GAMEPAD_F310_RIGHT_BUTTON)); + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemDriverStick.java b/src/main/java/org/mayheminc/util/MayhemDriverStick.java new file mode 100644 index 0000000..43963a6 --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemDriverStick.java @@ -0,0 +1,35 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj.Joystick; + +public class MayhemDriverStick { + + public final Joystick Joystick = new Joystick(Joysticks.DRIVER_JOYSTICK); + + public final Button DRIVER_STICK_BUTTON_ONE_DISABLED = new DisabledOnlyJoystickButton(Joystick, 1); + public final Button DRIVER_STICK_BUTTON_ONE_ENABLED = new EnabledOnlyJoystickButton(Joystick, 1); + public final Button DRIVER_STICK_BUTTON_TWO = new DisabledOnlyJoystickButton(Joystick, 2); + public final Button DRIVER_STICK_BUTTON_THREE = new DisabledOnlyJoystickButton(Joystick, 3); + public final Button DRIVER_STICK_BUTTON_FOUR = new DisabledOnlyJoystickButton(Joystick, 4); + public final Button DRIVER_STICK_BUTTON_FIVE = new DisabledOnlyJoystickButton(Joystick, 5); + public final Button DRIVER_STICK_BUTTON_SIX = new DisabledOnlyJoystickButton(Joystick, 6); + public final Button DRIVER_STICK_BUTTON_SEVEN = new DisabledOnlyJoystickButton(Joystick, 7); + public final Button DRIVER_STICK_BUTTON_EIGHT = new DisabledOnlyJoystickButton(Joystick, 8); + public final Button DRIVER_STICK_BUTTON_NINE = new JoystickButton(Joystick, 9); + public final Button DRIVER_STICK_BUTTON_TEN = new DisabledOnlyJoystickButton(Joystick, 10); + public final Button DRIVER_STICK_BUTTON_ELEVEN = new DisabledOnlyJoystickButton(Joystick, 11); + + public final Button DRIVER_STICK_ENA_BUTTON_SIX = new EnabledOnlyJoystickButton(Joystick, 6); + public final Button DRIVER_STICK_ENA_BUTTON_SEVEN = new EnabledOnlyJoystickButton(Joystick, 7); + public final Button DRIVER_STICK_ENA_BUTTON_TEN = new EnabledOnlyJoystickButton(Joystick, 10); + public final Button DRIVER_STICK_ENA_BUTTON_ELEVEN = new EnabledOnlyJoystickButton(Joystick, 11); + + public final int DRIVER_STICK_X_AXIS = 0; + public final int DRIVER_STICK_Y_AXIS = 1; + + public MayhemDriverStick() { + + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemFakeTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemFakeTalonSRX.java new file mode 100644 index 0000000..bb7b83b --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemFakeTalonSRX.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* 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 org.mayheminc.util; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +/** + * Replace a MayhemTalonSRX if you don't have hardware yet. + */ +public class MayhemFakeTalonSRX { + public MayhemFakeTalonSRX(int id) { + + } + + public void setNeutralMode(NeutralMode coast) { + } + + public void configNominalOutputVoltage(float f, float g) { + } + + public void configPeakOutputVoltage(double d, double e) { + } + + public void set(ControlMode percentoutput, double speed) { + } + + public double get() { + return 0; + } + + public void changeControlMode(ControlMode percentoutput) { + } + + public void config_kP(int i, double d, int j) { + } + + public void config_kI(int i, double d, int j) { + } + + public void config_kF(int i, double d, int j) { + } + + public void config_kD(int i, double d, int j) { + } + + public void configSelectedFeedbackSensor(FeedbackDevice quadencoder) { + } + + public void setPosition(int pos) { + } +} diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorPad.java b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java new file mode 100644 index 0000000..f10250a --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemOperatorPad.java @@ -0,0 +1,81 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; + +public class MayhemOperatorPad { + + public final class OPERATOR_PAD_AXIS { + public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; + public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; + public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; + public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + } + + public final Joystick OPERATOR_PAD = new Joystick(Joysticks.OPERATOR_GAMEPAD); + public final Button OPERATOR_PAD_BUTTON_ONE = new JoystickButton(OPERATOR_PAD, 1); + public final Button OPERATOR_PAD_BUTTON_TWO = new JoystickButton(OPERATOR_PAD, 2); + public final Button OPERATOR_PAD_BUTTON_THREE = new JoystickButton(OPERATOR_PAD, 3); + public final Button OPERATOR_PAD_BUTTON_FOUR = new JoystickButton(OPERATOR_PAD, 4); + public final Button OPERATOR_PAD_BUTTON_FIVE = new JoystickButton(OPERATOR_PAD, 5); + public final Button OPERATOR_PAD_BUTTON_SIX = new JoystickButton(OPERATOR_PAD, 6); + public final Button OPERATOR_PAD_BUTTON_SEVEN = new JoystickButton(OPERATOR_PAD, 7); + public final Button OPERATOR_PAD_BUTTON_EIGHT = new JoystickButton(OPERATOR_PAD, 8); + public final Button OPERATOR_PAD_BUTTON_NINE = new JoystickButton(OPERATOR_PAD, 9); + public final Button OPERATOR_PAD_BUTTON_TEN = new JoystickButton(OPERATOR_PAD, 10); + public final Button OPERATOR_PAD_BUTTON_ELEVEN = new JoystickButton(OPERATOR_PAD, 11); + public final Button OPERATOR_PAD_BUTTON_TWELVE = new JoystickButton(OPERATOR_PAD, 12); + public final Button FORCE_FIRE_BUTTON = new AndJoystickButton(OPERATOR_PAD, 5, OPERATOR_PAD, 7); + + public final JoystickPOVButton OPERATOR_PAD_D_PAD_LEFT = new JoystickPOVButton(OPERATOR_PAD, 270); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_RIGHT = new JoystickPOVButton(OPERATOR_PAD, 90); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_UP = new JoystickPOVButton(OPERATOR_PAD, 0); + public final JoystickPOVButton OPERATOR_PAD_D_PAD_DOWN = new JoystickPOVButton(OPERATOR_PAD, 180); + + // Operator Control Buttons + public final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_LEFT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_LEFT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_UP = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.NEGATIVE_ONLY); + public final JoystickAxisButton OPERATOR_PAD_RIGHT_Y_AXIS_DOWN = new JoystickAxisButton(OPERATOR_PAD, + OPERATOR_PAD_AXIS.OPERATOR_PAD_RIGHT_Y_AXIS, JoystickAxisButton.POSITIVE_ONLY); + + public static final int OPERATOR_PAD_LEFT_X_AXIS = 0; + public static final int OPERATOR_PAD_LEFT_Y_AXIS = 1; + public static final int OPERATOR_PAD_RIGHT_X_AXIS = 2; + public static final int OPERATOR_PAD_RIGHT_Y_AXIS = 3; + + private static final double Y_AXIS_DEAD_ZONE_PERCENT = 0.15; + private static final double X_AXIS_DEAD_ZONE_PERCENT = 0.15; + + public double getLeftYAxis() { + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_LEFT_Y_AXIS); + return ApplyDeadZone(value, Y_AXIS_DEAD_ZONE_PERCENT); + } + + public double getLeftXAxis() { + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_LEFT_X_AXIS); + return ApplyDeadZone(value, X_AXIS_DEAD_ZONE_PERCENT); + } + + public double getRightYAxis() { + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_RIGHT_Y_AXIS); + return ApplyDeadZone(value, Y_AXIS_DEAD_ZONE_PERCENT); + } + + public double getRightXAxis() { + double value = OPERATOR_PAD.getRawAxis(OPERATOR_PAD_LEFT_X_AXIS); + return ApplyDeadZone(value, X_AXIS_DEAD_ZONE_PERCENT); + } + + private double ApplyDeadZone(double value, double deadZone) { + + if (Math.abs(value) < deadZone) { + return 0; + } + return value; + } +} diff --git a/src/main/java/org/mayheminc/util/MayhemOperatorStick.java b/src/main/java/org/mayheminc/util/MayhemOperatorStick.java new file mode 100644 index 0000000..7746893 --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemOperatorStick.java @@ -0,0 +1,11 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.Joystick; + +public class MayhemOperatorStick { + + public final Joystick OPERATOR_STICK = new Joystick(Joysticks.OPERATOR_JOYSTICK); + + public MayhemOperatorStick() { + } +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/MayhemTalonSRX.java b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java new file mode 100644 index 0000000..8e6596d --- /dev/null +++ b/src/main/java/org/mayheminc/util/MayhemTalonSRX.java @@ -0,0 +1,138 @@ +package org.mayheminc.util; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.*; + +public class MayhemTalonSRX extends TalonSRX { + + public enum CurrentLimit { + HIGH_CURRENT, LOW_CURRENT + } + + ControlMode controlMode; + double p; + double i; + double d; + double f; + + public MayhemTalonSRX(int deviceNumber, CurrentLimit currentLimit) { + super(deviceNumber); + + this.configFactoryDefault(); + + this.configNominalOutputForward(0.0, 0); + this.configNominalOutputReverse(0.0, 0); + this.configPeakOutputForward(1.0, 0); + this.configPeakOutputReverse(-1.0, 0); + this.configVoltageCompSaturation(12.0); // "full output" scaled to 12.0V for all modes when enabled. + this.enableVoltageCompensation(true); // turn on voltage compensation + + this.setNeutralMode(NeutralMode.Coast); + + if (currentLimit == CurrentLimit.HIGH_CURRENT) { + this.configPeakCurrentLimit(60); + this.configContinuousCurrentLimit(40); + this.configPeakCurrentDuration(1000); + } else if (currentLimit == CurrentLimit.LOW_CURRENT) { + this.configContinuousCurrentLimit(30); + } + + // this.configContinuousCurrentLimit(0, 0); + // this.configPeakCurrentLimit(0, 0); + // this.configPeakCurrentDuration(0, 0); + // this.configForwardLimitSwitchSource(RemoteLimitSwitchSource.Deactivated, + // LimitSwitchNormal.Disabled, 0, 0); + // this.configForwardSoftLimitEnable(false, 0); + + // copied from CTRE Example: + // https://github.com/CrossTheRoadElec/Phoenix-Examples-Languages/blob/master/Java/Current%20Limit/src/org/usfirst/frc/team217/robot/Robot.java#L37 + // this.configPeakCurrentLimit(80, 10); + // this.configPeakCurrentDuration(60000, 10); /* this is a necessary call to + // avoid errata. */ + // this.configContinuousCurrentLimit(40, 10); + // this.enableCurrentLimit(true); /* honor initial setting */ + + } + + public ErrorCode config_kP(int slot, double value, int timeout) { + p = value; + return super.config_kP(slot, value, timeout); + } + + public ErrorCode config_kI(int slot, double value, int timeout) { + i = value; + return super.config_kI(slot, value, timeout); + } + + public ErrorCode config_kD(int slot, double value, int timeout) { + d = value; + return super.config_kD(slot, value, timeout); + } + + public ErrorCode config_kF(int slot, double value, int timeout) { + f = value; + return super.config_kF(slot, value, timeout); + } + + public double getP() { + return p; + } + + public double getI() { + return i; + } + + public double getD() { + return d; + } + + public double getF() { + return f; + } + + public void changeControlMode(ControlMode mode) { + controlMode = mode; + } + + public void set(int deviceID) { + this.set(controlMode, deviceID); + } + + public void setFeedbackDevice(FeedbackDevice feedback) { + this.configSelectedFeedbackSensor(feedback, 0, 0); + } + + public void configNominalOutputVoltage(float f, float g) { + this.configNominalOutputForward(f / 12.0, 1000); + this.configNominalOutputReverse(g / 12.0, 1000); + } + + public void configPeakOutputVoltage(double d, double e) { + this.configPeakOutputForward(d / 12.0, 1000); + this.configPeakOutputReverse(e / 12.0, 1000); + + } + + public double getError() { + return this.getClosedLoopError(0); + } + + public void setPosition(int zeroPositionCount) { + this.setSelectedSensorPosition(zeroPositionCount, 0, 1000); + } + + public int getPosition() { + return this.getSelectedSensorPosition(0); + } + + public double getSpeed() { + return this.getSelectedSensorVelocity(0); + } + + public void setEncPosition(int i) { + setPosition(i); + } +} diff --git a/src/main/java/org/mayheminc/util/ObjectListener.java b/src/main/java/org/mayheminc/util/ObjectListener.java new file mode 100644 index 0000000..5cae99b --- /dev/null +++ b/src/main/java/org/mayheminc/util/ObjectListener.java @@ -0,0 +1,159 @@ +package org.mayheminc.util; + +import java.util.*; +import java.io.*; +import java.net.*; +import java.nio.*; + +public class ObjectListener extends Thread { + protected static final int MAX_OBJECTS_PER_FRAME = 20; + protected static final int MAYHEM_MAGIC = 0x1519B0B4; + protected static final int MAX_BUFFER = 1500; + protected static final int DEFAULT_PORT = 5810; + + private DatagramSocket socket; + private DatagramPacket packet; + private ByteBuffer buffer; + private int lastFrame = 0; + private ArrayList objList; + private Callback callback = null; + + public interface Callback { + public void objectListenerCallback(int frame, ArrayList objList); + } + + public ObjectListener() throws SocketException { + this(DEFAULT_PORT); + } + + public ObjectListener(int port) throws SocketException { + super("ObjectListener-" + port); + + socket = new DatagramSocket(port); + + byte[] byteBuffer = new byte[MAX_BUFFER]; + packet = new DatagramPacket(byteBuffer, byteBuffer.length); + buffer = ByteBuffer.wrap(byteBuffer); + + objList = new ArrayList(); + } + + public int getLastFrame() { + return lastFrame; + } + + public List getObjectList() { + return objList; + } + + public void setCallback(Callback callback) { + this.callback = callback; + } + + public void run() { + String name = super.getName(); + long lastTimestamp = 0; + + while (true) { + // Receive new datagram + try { + socket.receive(packet); + buffer.rewind(); + } catch (IOException e) { + System.err.println(super.getName() + " encountered an error"); + e.printStackTrace(); + System.err.println(super.getName() + " aborting"); + break; + } + + // Abort if told to do so + if (Thread.interrupted()) + break; + + // Validate packet + int magic = buffer.getInt(); + if (magic != MAYHEM_MAGIC) { + System.err.println(name + ": invalid packet received (magic == 0x" + Integer.toHexString(magic) + ")"); + continue; + } + + // Get information about the update + int frame = buffer.getInt(); + long timestamp = buffer.getLong(); + + // Check for out-of-date data + if (timestamp <= lastTimestamp) { + System.err.println(name + ": timestamp for new frame #" + frame + " (" + timestamp + ") is not newer than that for previous frame #" + lastFrame + " (" + lastTimestamp + "); rejecting out-of-date data"); + lastTimestamp = timestamp; + continue; + } + if (frame <= lastFrame) { + System.err.println(name + ": frame #" + frame + " is earlier than existing frame #" + lastFrame + "; did object detection service restart?"); + } + + // Get list of all objects involved + ArrayList objList = new ArrayList(); + for (int i = 0; i < MAX_OBJECTS_PER_FRAME; i++) { + // Pull the next object from the buffer + ObjectLocation loc = new ObjectLocation(buffer); + + // As soon as one object is none, so are the rest + if (loc.type == ObjectLocation.ObjectTypes.OBJ_NONE) + break; + + // Add object to our list + objList.add(loc); + } + + // Update the list of objects + this.objList = objList; + lastFrame = frame; + lastTimestamp = timestamp; + + // Invoke callback, if applicable + if (callback != null) { + callback.objectListenerCallback(frame, objList); + } + } + + // Clean up + socket.close(); + } + + // Sample implementation for testing and demonstration purposes + public static void main(String[] args) { + ObjectListener listener; + Callback callback = new ObjectListener.Callback() { + public void objectListenerCallback(int frame, ArrayList objList) { + System.out.println("Received notification about objects in frame #" + frame); + for (ObjectLocation loc: objList) { + System.out.println(" " + loc); + } + } + }; + + // Create the listener + try { + listener = new ObjectListener(); + } catch (SocketException e) { + e.printStackTrace(); + return; + } + + // Use the callback implementation + listener.setCallback(callback); + + // Begin listening + System.out.println("Starting object listener...\n"); + listener.start(); + + // Wait forever -- notifications will come from callback + while (true) { + try { + Thread.sleep(600); + } catch (InterruptedException e) { + break; + } + } + } +} diff --git a/src/main/java/org/mayheminc/util/ObjectLocation.java b/src/main/java/org/mayheminc/util/ObjectLocation.java new file mode 100644 index 0000000..a903461 --- /dev/null +++ b/src/main/java/org/mayheminc/util/ObjectLocation.java @@ -0,0 +1,58 @@ +package org.mayheminc.util; + +import java.nio.*; +import java.text.DecimalFormat; + +public class ObjectLocation { + public enum ObjectTypes { + OBJ_NONE, + OBJ_CUBE, + OBJ_SCALE_CENTER, + OBJ_SCALE_BLUE, + OBJ_SCALE_RED, + OBJ_SWITCH_RED, + OBJ_SWITCH_BLUE, + OBJ_PORTAL_RED, + OBJ_PORTAL_BLUE, + OBJ_EXCHANGE_RED, + OBJ_EXCHANGE_BLUE, + OBJ_BUMPERS_RED, + OBJ_BUMPERS_BLUE, + OBJ_BUMPERS_CLASS13, + OBJ_BUMPERS_CLASS14, + OBJ_BUMPERS_CLASS15, + OBJ_BUMPERS_CLASS16, + OBJ_BUMPERS_CLASS17, + OBJ_BUMPERS_CLASS18, + OBJ_BUMPERS_CLASS19, + OBJ_BUMPERS_CLASS20, + OBJ_EOL, + }; + + public ObjectTypes type; + public float x; + public float y; + public float width; + public float height; + public float probability; + + public ObjectLocation() { + type = ObjectTypes.OBJ_NONE; + x = y = width = height = probability = 0; + } + + public ObjectLocation(ByteBuffer buffer) { + type = ObjectTypes.values()[buffer.getInt()]; + x = (float)buffer.getInt() / Integer.MAX_VALUE; + y = (float)buffer.getInt() / Integer.MAX_VALUE; + width = (float)buffer.getInt() / Integer.MAX_VALUE; + height = (float)buffer.getInt() / Integer.MAX_VALUE; + probability = (float)buffer.getInt() / Integer.MAX_VALUE; + } + + public String toString() { + DecimalFormat df = new DecimalFormat("0.00"); + + return type.name() + "@" + df.format(x) + "x" + df.format(y) + "+" + df.format(width) + "x" + df.format(height) + "[" + df.format(probability * 100) + "%]"; + } +} diff --git a/src/main/java/org/mayheminc/util/PidCycle.java b/src/main/java/org/mayheminc/util/PidCycle.java new file mode 100644 index 0000000..cb5fc71 --- /dev/null +++ b/src/main/java/org/mayheminc/util/PidCycle.java @@ -0,0 +1,10 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class PidCycle extends InstantCommand { + public void initialize() { + // Robot.pidTuner.cyclePid(); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/PidTuner.java b/src/main/java/org/mayheminc/util/PidTuner.java new file mode 100644 index 0000000..52b952e --- /dev/null +++ b/src/main/java/org/mayheminc/util/PidTuner.java @@ -0,0 +1,171 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.button.Button; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * This is a class to help tune a PID. Another class that extends + * IPidTunerObject is passed in. This interface defines methods that are used to + * set the P-I-D values. This class gets joystick buttons that define the + * different options. - Button 1: cycle through P-I-D-F values - Button 2: + * increase the value. - Button 3: decrease the value. - Button 4: cycle through + * the amount the value changes: 10, 1, .1, .01, .001, .0001 This class needs to + * be called from updateSmartDashboard(). + * + * @author User + * + */ +public class PidTuner extends InstantCommand implements Subsystem { + Button m_PidCycle; + Button m_Inc; + Button m_Dec; + Button m_ValueCycle; + PidTunerObject m_pidObj; + + enum PidCycle { + P, I, D, F + }; + + int m_cycle; + int m_position; + + // Remember the buttons and call this object when they are pressed. + // remember the PID Object so we can get/set the PID values. + public PidTuner(final Button b1, final Button b2, final Button b3, final Button b4, final PidTunerObject obj) { + + CommandScheduler.getInstance().registerSubsystem(this); + + b1.whenPressed(this); + b2.whenPressed(this); + b3.whenPressed(this); + b4.whenPressed(this); + + m_PidCycle = b1; + m_ValueCycle = b2; + m_Inc = b3; + m_Dec = b4; + + m_pidObj = obj; + } + + public boolean runsWhenDisabled() { + return true; + } + + // Run the 'instant command'. Determine which command was pressed. + public void initialize() { + if (m_PidCycle.get()) { + RunCycle(); + } + if (m_Inc.get()) { + RunInc(); + } + if (m_Dec.get()) { + RunDec(); + } + if (m_ValueCycle.get()) { + RunValue(); + } + } + + // set the P, I, or D or F that we are changing + public void RunCycle() { + // System.out.println("PID Tuner: RunCycle"); + m_cycle++; + m_cycle = m_cycle % 4; + } + + String getCycleStr() { + final String str[] = { "P", "I", "D", "F" }; + + return str[m_cycle]; + } + + // calculate the amount to increment, get the value, apply the amount, set the + // new value + public void RunInc() { + // System.out.println("PID Tuner: RunInc"); + final double amount = calculateAmount(); + + double value = getValue(); + value = value + amount; + setValue(value); + } + + // if m_position is 1, return 10, 0 returns 1, -1 returns 0.1, etc. + double calculateAmount() { + final double retval = Math.pow(10, m_position); + return retval; + } + + // based on the cycle, get the P, I, or D. + double getValue() { + switch (m_cycle) { + case 0: + return m_pidObj.getP(); + case 1: + return m_pidObj.getI(); + case 2: + return m_pidObj.getD(); + case 3: + return m_pidObj.getF(); + } + return 0.0; + } + + // based on the cycle, set the P, I, or D or F. + void setValue(final double d) { + switch (m_cycle) { + case 0: + m_pidObj.setP(d); + break; + case 1: + m_pidObj.setI(d); + break; + case 2: + m_pidObj.setD(d); + break; + case 3: + m_pidObj.setF(d); + break; + } + } + + // calculate the amount to decrement, get the value, apply the amount, set the + // new value + public void RunDec() { + // System.out.println("PID Tuner: RunDec"); + final double amount = calculateAmount(); + + double value = getValue(); + value = value - amount; + setValue(value); + } + + // Change the amount we are decrementing or incrementing + public void RunValue() { + // System.out.println("PID Tuner: RunValue"); + + m_position--; + if (m_position < -4) { + m_position = 1; // 10.1234 + } + } + + @Override + public void periodic() { + updateSmartDashboard(); + } + + private void updateSmartDashboard() { + SmartDashboard.putNumber("PID Tuner P", m_pidObj.getP()); + SmartDashboard.putNumber("PID Tuner I", m_pidObj.getI()); + SmartDashboard.putNumber("PID Tuner D", m_pidObj.getD()); + SmartDashboard.putNumber("PID Tuner F", m_pidObj.getF()); + + SmartDashboard.putString("PID Tuner Cycle", getCycleStr()); + SmartDashboard.putNumber("PID Tuner Amount", calculateAmount()); + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/PidTunerObject.java b/src/main/java/org/mayheminc/util/PidTunerObject.java new file mode 100644 index 0000000..1c77ded --- /dev/null +++ b/src/main/java/org/mayheminc/util/PidTunerObject.java @@ -0,0 +1,20 @@ +package org.mayheminc.util; + +public interface PidTunerObject { + + double getP(); + + double getI(); + + double getD(); + + double getF(); + + void setP(double d); + + void setI(double d); + + void setD(double d); + + void setF(double d); +} diff --git a/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java b/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java new file mode 100644 index 0000000..061fcbc --- /dev/null +++ b/src/main/java/org/mayheminc/util/RangeFinder_GP2D120.java @@ -0,0 +1,60 @@ +package org.mayheminc.util; + +import edu.wpi.first.wpilibj.AnalogInput; + +// http://media.digikey.com/pdf/Data%20Sheets/Sharp%20PDFs/GP2D120.pdf + +public class RangeFinder_GP2D120 { + + private AnalogInput m_channel; + private static final int HISTORY_BUFFER_SIZE = 5; + private double m_historyBuffer[]=new double[HISTORY_BUFFER_SIZE]; + private int m_historyBufferIndex; + private double m_filteredVoltage; + + public RangeFinder_GP2D120(int channel, int index) { + m_channel = new AnalogInput(channel); + m_filteredVoltage = 0.0; + } + + public void periodic() { + double min = m_historyBuffer[0]; + m_historyBuffer[m_historyBufferIndex]= m_channel.getVoltage(); //return channel.getVoltage(); + m_historyBufferIndex++; + + if (m_historyBufferIndex>=HISTORY_BUFFER_SIZE) { + m_historyBufferIndex=0; + } + + for (int i=1; i= OBJECT_IS_SEEN_VOLTAGE; + } + + // had been 0.80 prior to 31 Jan + private static final double OBJECT_IS_CLOSE_VOLTAGE = 0.80; + public boolean isObjectClose() { + return getFilteredVoltage() >= OBJECT_IS_CLOSE_VOLTAGE; + } + + // added on 11 Feb 2015 to allow "floating off tote" + private static final double OBJECT_IS_VERY_CLOSE_VOLTAGE = 1.60; + public boolean isObjectVeryClose() { + return getFilteredVoltage() >= OBJECT_IS_VERY_CLOSE_VOLTAGE; + } + +} \ No newline at end of file diff --git a/src/main/java/org/mayheminc/util/SchedulerManager.java b/src/main/java/org/mayheminc/util/SchedulerManager.java new file mode 100644 index 0000000..26300ab --- /dev/null +++ b/src/main/java/org/mayheminc/util/SchedulerManager.java @@ -0,0 +1,107 @@ +/** + * Copyright (c) 2015-2019 Team 501 - The PowerKnights. All Rights Reserved. + * Open Source Software - May be modified and shared by FRC teams. The code must + * be accompanied by the Team 501 BSD license file in the root directory of the + * project. You may also obtain a copy of it from the following: + * http://www.opensource.org/licenses/bsd-license.php. + * + * See (Git) repository metadata for author and revision history for this file. + **/ + +package org.mayheminc.util; + +// import org.slf4j.Logger; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableType; +import edu.wpi.first.networktables.NetworkTableValue; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +// import riolog.RioLogger; + +/** + * + */ +public class SchedulerManager // implements ITelemetryProvider +{ + + /* Our classes logger */ + // @SuppressWarnings("unused") + // private static final Logger logger = + // RioLogger.getLogger(SchedulerManager.class.getName()); + + /** Singleton instance of class for all to use **/ + private static SchedulerManager ourInstance; + /** Name of our subsystem **/ + private final static String myName = "Running Commands";// TelemetryNames.Scheduler.name; + + /** + * Constructs instance of the subsystem. Assumed to be called before any usage + * of the subsystem; and verifies only called once. Allows controlled startup + * sequencing of the robot and all it's subsystems. + **/ + public static synchronized void constructInstance() { + // SmartDashboard.putBoolean(TelemetryNames.Scheduler.status, false); + + if (ourInstance != null) { + throw new IllegalStateException(myName + " Already Constructed"); + } + ourInstance = new SchedulerManager(); + + // SmartDashboard.putBoolean(TelemetryNames.Scheduler.status, true); + } + + /** + * Returns the singleton instance of the subsystem in the form of the + * Interface that is defined for it. If it hasn't been constructed yet, + * throws an IllegalStateException. + * + * @return singleton instance of subsystem + **/ + public static SchedulerManager getInstance() { + if (ourInstance == null) { + throw new IllegalStateException(myName + " Not Constructed Yet"); + } + return ourInstance; + } + + private final NetworkTable liveWindow; + private final NetworkTable ungrouped; + private final NetworkTable scheduler; + private final StringBuilder names; + + public SchedulerManager() { + NetworkTableInstance inst = NetworkTableInstance.getDefault(); + liveWindow = inst.getTable("LiveWindow"); + ungrouped = liveWindow.getSubTable("Ungrouped"); + scheduler = ungrouped.getSubTable("Scheduler"); + + names = new StringBuilder(); + } + + // @Override + public void updateTelemetry() { + NetworkTableEntry namesEntry = scheduler.getEntry("Names"); + NetworkTableValue namesValue = namesEntry.getValue(); + if ((namesValue == null)) { + SmartDashboard.putString("Current Commands", "namesValue is null"); + return; + } + + if ((namesValue.getType() != NetworkTableType.kStringArray)) { + + SmartDashboard.putString("Current Commands", "namesValue is " + namesValue.getType()); + return; + } + + names.setLength(0); + for (String name : namesValue.getStringArray()) { + names.append(name).append(": "); + } + + SmartDashboard.putString("Current Commands", names.toString()); + } + +} diff --git a/src/main/java/org/mayheminc/util/Utils.java b/src/main/java/org/mayheminc/util/Utils.java new file mode 100644 index 0000000..edcabda --- /dev/null +++ b/src/main/java/org/mayheminc/util/Utils.java @@ -0,0 +1,262 @@ +/* + * Utility class with useful methods in it + */ +package org.mayheminc.util; + +/** + * + * @author Team1519 + */ +public class Utils { + + public static double oneDecimalPlace(double d) { + return ((double) ((int) (d * 10))) / 10; + } + + public static double twoDecimalPlaces(double d) { + return ((double) ((int) (d * 100))) / 100; + } + + public static double threeDecimalPlaces(double d) { + return ((double) ((int) (d * 1000))) / 1000; + } + + public static double fourDecimalPlaces(double d) { + return ((double) ((int) (d * 10000))) / 10000; + } + + /** + * Returns the arc tangent of an angle, in the range of + * -Math.PI/2 through + * Math.PI/2. Special cases:

A result must be within 1 + * ulp of the correctly rounded result. Results must be semi-monotonic. + * + * @param a - the value whose arc tangent is to be returned. + * @return the arc tangent of the argument. + */ + public static double atan(double a) { + // Special cases. + if (Double.isNaN(a)) { + return Double.NaN; + } + + if (a == 0.0) { + return a; + } + + // Compute the arc tangent. + boolean negative = false; + boolean greaterThanOne = false; + int i = 0; + + if (a < 0.0) { + a = -a; + negative = true; + } + + if (a > 1.0) { + a = 1.0 / a; + greaterThanOne = true; + } + + double t; + + for (; a > PIover12; a *= t) { + i++; + t = a + ATAN_CONSTANT; + t = 1.0 / t; + a *= ATAN_CONSTANT; + a--; + } + + double aSquared = a * a; + + double arcTangent = aSquared + 1.4087812; + arcTangent = 0.55913709 / arcTangent; + arcTangent += 0.60310578999999997; + arcTangent -= 0.051604539999999997 * aSquared; + arcTangent *= a; + + for (; i > 0; i--) { + arcTangent += PIover6; + } + + if (greaterThanOne) { + arcTangent = PIover2 - arcTangent; + } + + if (negative) { + arcTangent = -arcTangent; + } + + return arcTangent; + } + /** + * Constant for PI divided by 2. + */ + public static final double PIover2 = Math.PI / 2; + /** + * Constant for PI divided by 4. + */ + public static final double PIover4 = Math.PI / 4; + /** + * Constant for PI divided by 6. + */ + public static final double PIover6 = Math.PI / 6; + /** + * Constant for PI divided by 12. + */ + public static final double PIover12 = Math.PI / 12; + /** + * Constant used in the + * atan calculation. + */ + private static final double ATAN_CONSTANT = 1.732050807569; + + /** + * Converts rectangular coordinates (x, y) to polar (r, theta). This + * method computes the phase theta by computing an arc tangent of y/x + * in the range of -pi to pi. Special cases:

A result must be within 2 ulps of the correctly rounded result. + * Results must be semi-monotonic. + * + * @param y - the ordinate coordinate + * @param x - the abscissa coordinate + * @return the theta component of the point (r, theta) in + * polar coordinates that corresponds to the point (x, y) in Cartesian + * coordinates. + */ + public static double atan2(double y, double x) { + // Special cases. + if (Double.isNaN(y) || Double.isNaN(x)) { + return Double.NaN; + } else if (Double.isInfinite(y)) { + if (y > 0.0) // Positive infinity + { + if (Double.isInfinite(x)) { + if (x > 0.0) { + return PIover4; + } else { + return 3.0 * PIover4; + } + } else if (x != 0.0) { + return PIover2; + } + } else // Negative infinity + { + if (Double.isInfinite(x)) { + if (x > 0.0) { + return -PIover4; + } else { + return -3.0 * PIover4; + } + } else if (x != 0.0) { + return -PIover2; + } + } + } else if (y == 0.0) { + if (x > 0.0) { + return y; + } else if (x < 0.0) { + return Math.PI; + } + else { + return 0; + } + } else if (Double.isInfinite(x)) { + if (x > 0.0) // Positive infinity + { + if (y > 0.0) { + return 0.0; + } else if (y < 0.0) { + return -0.0; + } + } else // Negative infinity + { + if (y > 0.0) { + return Math.PI; + } else if (y < 0.0) { + return -Math.PI; + } + } + } else if (x == 0.0) { + if (y > 0.0) { + return PIover2; + } else if (y < 0.0) { + return -PIover2; + } + else { + // Should not get her with y test above + return 0; + } + } + + + // Implementation a simple version ported from a PASCAL implementation: + // http://everything2.com/index.pl?node_id=1008481 + + double arcTangent; + + // Use arctan() avoiding division by zero. + if (Math.abs(x) > Math.abs(y)) { + arcTangent = atan(y / x); + } else { + arcTangent = atan(x / y); // -PI/4 <= a <= PI/4 + + if (arcTangent < 0) { + arcTangent = -PIover2 - arcTangent; // a is negative, so we're adding + } else { + arcTangent = PIover2 - arcTangent; + } + } + + // Adjust result to be from [-PI, PI] + if (x < 0) { + if (y < 0) { + arcTangent = arcTangent - Math.PI; + } else { + arcTangent = arcTangent + Math.PI; + } + } + + return arcTangent; + } + + public static boolean isBetween(double value, double upper, double lower){ + return value < upper && value > lower; + } +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..aa5554e --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,180 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.18.2", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.18.2" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.18.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.18.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.18.2", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.18.2", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.18.2", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "diagnostics", + "version": "5.18.2", + "libName": "CTRE_PhoenixDiagnostics", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.18.2", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.18.2", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.18.2", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json new file mode 100644 index 0000000..d7bd9b0 --- /dev/null +++ b/vendordeps/WPILibNewCommands.json @@ -0,0 +1,37 @@ +{ + "fileName": "WPILibNewCommands.json", + "name": "WPILib-New-Commands", + "version": "2020.0.0", + "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", + "mavenUrls": [], + "jsonUrl": "", + "javaDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-java", + "version": "wpilib" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "edu.wpi.first.wpilibNewCommands", + "artifactId": "wpilibNewCommands-cpp", + "version": "wpilib", + "libName": "wpilibNewCommands", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxaarch64bionic", + "windowsx86-64", + "windowsx86", + "linuxx86-64", + "osxx86-64" + ] + } + ] +} diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json new file mode 100644 index 0000000..dca1d82 --- /dev/null +++ b/vendordeps/navx_frc.json @@ -0,0 +1,35 @@ +{ + "fileName": "navx_frc.json", + "name": "KauaiLabs_navX_FRC", + "version": "3.1.413", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "mavenUrls": [ + "https://repo1.maven.org/maven2/" + ], + "jsonUrl": "https://www.kauailabs.com/dist/frc/2020/navx_frc.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-java", + "version": "3.1.413" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-cpp", + "version": "3.1.413", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file