diff --git a/2019robot/.gitignore b/2019robot/.gitignore deleted file mode 100644 index 61f25ce..0000000 --- a/2019robot/.gitignore +++ /dev/null @@ -1,160 +0,0 @@ -# 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 - -# Log file -*.log - -# BlueJ files -*.ctxt - -# Mobile Tools for Java (J2ME) -.mtj.tmp/ - -# Package Files # -*.jar -*.war -*.nar -*.ear -*.zip -*.tar.gz -*.rar - -# 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/ - - -# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/2019robot/.vscode/launch.json b/2019robot/.vscode/launch.json deleted file mode 100644 index c9c9713..0000000 --- a/2019robot/.vscode/launch.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - // 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/2019robot/.vscode/settings.json b/2019robot/.vscode/settings.json deleted file mode 100644 index 860e319..0000000 --- a/2019robot/.vscode/settings.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "java.configuration.updateBuildConfiguration": "automatic", - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - ".classpath": true, - ".project": true - }, - "wpilib.online": true -} diff --git a/2019robot/.wpilib/wpilib_preferences.json b/2019robot/.wpilib/wpilib_preferences.json deleted file mode 100644 index 95c6671..0000000 --- a/2019robot/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1,6 +0,0 @@ -{ - "enableCppIntellisense": false, - "currentLanguage": "java", - "projectYear": "2019", - "teamNumber": 4388 -} \ No newline at end of file diff --git a/2019robot/build.gradle b/2019robot/build.gradle deleted file mode 100644 index ea2312b..0000000 --- a/2019robot/build.gradle +++ /dev/null @@ -1,61 +0,0 @@ -plugins { - id "java" - id "edu.wpi.first.GradleRIO" version "2019.1.1" -} - -def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.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 = true - -// Maven central needed for JUnit -repositories { - mavenCentral() -} - -// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. -// Also defines JUnit 4. -dependencies { - compile wpi.deps.wpilib() - compile wpi.deps.vendor.java() - nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) - nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) - testCompile 'junit:junit:4.12' -} - -// 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.compile.collect { it.isDirectory() ? it : zipTree(it) } } - manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) -} diff --git a/2019robot/gradle/wrapper/gradle-wrapper.jar b/2019robot/gradle/wrapper/gradle-wrapper.jar deleted file mode 100644 index 457aad0..0000000 Binary files a/2019robot/gradle/wrapper/gradle-wrapper.jar and /dev/null differ diff --git a/2019robot/gradle/wrapper/gradle-wrapper.properties b/2019robot/gradle/wrapper/gradle-wrapper.properties deleted file mode 100644 index d08253c..0000000 --- a/2019robot/gradle/wrapper/gradle-wrapper.properties +++ /dev/null @@ -1,5 +0,0 @@ -distributionBase=GRADLE_USER_HOME -distributionPath=permwrapper/dists -distributionUrl=https\://services.gradle.org/distributions/gradle-5.0-bin.zip -zipStoreBase=GRADLE_USER_HOME -zipStorePath=permwrapper/dists diff --git a/2019robot/gradlew b/2019robot/gradlew deleted file mode 100644 index af6708f..0000000 --- a/2019robot/gradlew +++ /dev/null @@ -1,172 +0,0 @@ -#!/usr/bin/env sh - -############################################################################## -## -## 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"' - -# 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, switch paths to Windows format before running java -if $cygwin ; 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=$((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" - -# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong -if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then - cd "$(dirname "$0")" -fi - -exec "$JAVACMD" "$@" diff --git a/2019robot/gradlew.bat b/2019robot/gradlew.bat deleted file mode 100644 index 6d57edc..0000000 --- a/2019robot/gradlew.bat +++ /dev/null @@ -1,84 +0,0 @@ -@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" - -@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/2019robot/settings.gradle b/2019robot/settings.gradle deleted file mode 100644 index b0f4d48..0000000 --- a/2019robot/settings.gradle +++ /dev/null @@ -1,25 +0,0 @@ -import org.gradle.internal.os.OperatingSystem - -pluginManagement { - repositories { - mavenLocal() - gradlePluginPortal() - String frcYear = '2019' - File frcHome - if (OperatingSystem.current().isWindows()) { - String publicFolder = System.getenv('PUBLIC') - if (publicFolder == null) { - publicFolder = "C:\\Users\\Public" - } - frcHome = new File(publicFolder, "frc${frcYear}") - } else { - def userFolder = System.getProperty("user.home") - frcHome = new File(userFolder, "frc${frcYear}") - } - def frcHomeMaven = new File(frcHome, 'maven') - maven { - name 'frcHome' - url frcHomeMaven - } - } -} diff --git a/2019robot/src/main/deploy/example.txt b/2019robot/src/main/deploy/example.txt deleted file mode 100644 index 70c79b6..0000000 --- a/2019robot/src/main/deploy/example.txt +++ /dev/null @@ -1,3 +0,0 @@ -Files placed in this directory will be deployed to the RoboRIO into the -'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function -to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/2019robot/src/main/java/buttons/XBoxTriggerButton.java b/2019robot/src/main/java/buttons/XBoxTriggerButton.java deleted file mode 100644 index 7abfe7c..0000000 --- a/2019robot/src/main/java/buttons/XBoxTriggerButton.java +++ /dev/null @@ -1,61 +0,0 @@ -package buttons; - -import org.usfirst.frc4388.controller.XboxController; - -import edu.wpi.first.wpilibj.buttons.Button; - -public class XBoxTriggerButton extends Button -{ - public static final int RIGHT_TRIGGER = 0; - public static final int LEFT_TRIGGER = 1; - public static final int RIGHT_AXIS_UP_TRIGGER = 2; - public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; - public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; - public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; - public static final int LEFT_AXIS_UP_TRIGGER = 6; - public static final int LEFT_AXIS_DOWN_TRIGGER = 7; - public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; - public static final int LEFT_AXIS_LEFT_TRIGGER = 9; - - private XboxController m_controller; - private int m_trigger; - - public XBoxTriggerButton(XboxController controller, int trigger) { - m_controller = controller; - m_trigger = trigger; - } - - public boolean get() { - if (m_trigger == RIGHT_TRIGGER) { - return m_controller.getRightTrigger(); - } - else if (m_trigger == LEFT_TRIGGER) { - return m_controller.getLeftTrigger(); - } - else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { - return m_controller.getRightAxisUpTrigger(); - } - else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { - return m_controller.getRightAxisDownTrigger(); - } - else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { - return m_controller.getRightAxisRightTrigger(); - } - else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { - return m_controller.getRightAxisLeftTrigger(); - } - else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { - return m_controller.getLeftAxisUpTrigger(); - } - else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { - return m_controller.getLeftAxisDownTrigger(); - } - else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { - return m_controller.getLeftAxisRightTrigger(); - } - else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { - return m_controller.getLeftAxisLeftTrigger(); - } - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java b/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java deleted file mode 100644 index d8f08dc..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java +++ /dev/null @@ -1,12 +0,0 @@ -package org.usfirst.frc4388.controller; - -public interface IHandController { - - public double getLeftXAxis(); - - public double getLeftYAxis(); - - public double getRightXAxis(); - - public double getRightYAxis(); -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java b/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java deleted file mode 100644 index 040282a..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java +++ /dev/null @@ -1,205 +0,0 @@ - -package org.usfirst.frc4388.controller; - -import edu.wpi.first.wpilibj.Joystick; - -/** - * This is a wrapper for the WPILib Joystick class that represents an XBox - * controller. - * @author frc1675 - */ -public class XboxController implements IHandController -{ - public static final int LEFT_X_AXIS = 0; - public static final int LEFT_Y_AXIS = 1; - public static final int LEFT_TRIGGER_AXIS = 2; - public static final int RIGHT_TRIGGER_AXIS = 3; - public static final int RIGHT_X_AXIS = 4; - public static final int RIGHT_Y_AXIS = 5; - public static final int LEFT_RIGHT_DPAD_AXIS = 6; - public static final int TOP_BOTTOM_DPAD_AXIS = 6; - - public static final int A_BUTTON = 1; - public static final int B_BUTTON = 2; - public static final int X_BUTTON = 3; - public static final int Y_BUTTON = 4; - public static final int LEFT_BUMPER_BUTTON = 5; - public static final int RIGHT_BUMPER_BUTTON = 6; - public static final int BACK_BUTTON = 7; - public static final int START_BUTTON = 8; - - public static final int LEFT_JOYSTICK_BUTTON = 9; - public static final int RIGHT_JOYSTICK_BUTTON = 10; - - private static final double LEFT_DPAD_TOLERANCE = -0.9; - private static final double RIGHT_DPAD_TOLERANCE = 0.9; - private static final double BOTTOM_DPAD_TOLERANCE = -0.9; - private static final double TOP_DPAD_TOLERANCE = 0.9; - - private static final double LEFT_TRIGGER_TOLERANCE = 0.5; - private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; - - private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; - private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; - - private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; - private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; - - private static final double DEADZONE = 0.1; - - private Joystick stick; - - public XboxController(int portNumber){ - stick = new Joystick(portNumber); - } - - public Joystick getJoyStick() { - return stick; - } - - private boolean inDeadZone(double input){ - boolean inDeadZone; - if(Math.abs(input) < DEADZONE){ - inDeadZone = true; - }else{ - inDeadZone = false; - } - return inDeadZone; - } - - private double getAxisWithDeadZoneCheck(double input){ - if(inDeadZone(input)){ - input = 0.0; - } - return input; - } - - public boolean getAButton(){ - return stick.getRawButton(A_BUTTON); - } - - public boolean getXButton(){ - return stick.getRawButton(X_BUTTON); - } - - public boolean getBButton(){ - return stick.getRawButton(B_BUTTON); - } - - public boolean getYButton(){ - return stick.getRawButton(Y_BUTTON); - } - - public boolean getBackButton(){ - return stick.getRawButton(BACK_BUTTON); - } - - public boolean getStartButton(){ - return stick.getRawButton(START_BUTTON); - } - - public boolean getLeftBumperButton(){ - return stick.getRawButton(LEFT_BUMPER_BUTTON); - } - - public boolean getRightBumperButton(){ - return stick.getRawButton(RIGHT_BUMPER_BUTTON); - } - - public boolean getLeftJoystickButton(){ - return stick.getRawButton(LEFT_JOYSTICK_BUTTON); - } - - public boolean getRightJoystickButton(){ - return stick.getRawButton(RIGHT_JOYSTICK_BUTTON); - } - - public double getLeftXAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_X_AXIS)); - } - - public double getLeftYAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_Y_AXIS)); - } - - public double getRightXAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_X_AXIS)); - } - - public double getRightYAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_Y_AXIS)); - } - - public double getLeftTriggerAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_TRIGGER_AXIS)); - } - - public double getRightTriggerAxis(){ - return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_TRIGGER_AXIS)); - } - - /**Returns -1 if nothing is pressed, or the angle of the button pressed 0 = up, 90 = right, etc.*/ - public int getDpadAngle() { - return stick.getPOV(); - } - - public boolean getDPadLeft(){ - return (stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); - } - - public boolean getDPadRight(){ - return (stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); - } - - public boolean getDPadTop(){ - return (stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); - } - - /*public boolean getDPadBottom(){ - return (stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); - } -*/ - public boolean getLeftTrigger(){ - return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); - } - - public boolean getRightTrigger(){ - return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); - } - - public boolean getRightAxisUpTrigger(){ - return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); - } - - public boolean getRightAxisDownTrigger(){ - return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); - } - - public boolean getRightAxisLeftTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); - } - - public boolean getRightAxisRightTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); - } - - public boolean getLeftAxisUpTrigger(){ - return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); - } - - public boolean getLeftAxisDownTrigger(){ - return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); - } - - public boolean getLeftAxisLeftTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); - } - - public boolean getLeftAxisRightTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java deleted file mode 100644 index f6a18a1..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java +++ /dev/null @@ -1,72 +0,0 @@ - -package org.usfirst.frc4388.robot; - - -/** - * A list of constants used by the rest of the robot code. This include physics - * constants as well as constants determined through calibrations. - */ - -public class Constants { - - public static double kDriveWheelDiameterInches = 6.04; - public static double kTrackLengthInches = 10; - public static double kTrackWidthInches = 26.5; - public static double kTrackEffectiveDiameter = (kTrackWidthInches * kTrackWidthInches + kTrackLengthInches * kTrackLengthInches) / kTrackWidthInches; - public static double kTrackScrubFactor = 0.75; - - // Drive constants - public static double kDriveLowGearMaxSpeedInchesPerSec = 12.0 * 5.0; - public static double kDriveStraightBasicMaxSpeedInchesPerSec = 72.0; - public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0; - public static double kDriveStraightBasicYawErrorDivisor = 20.0; // steer parameter will be -yawError divided by this - public static double kDriveStraightBasicMaxSteerMagnitude = 0.8; // keep absolute value of steer parameter below this - public static double kDriveTurnBasicTankMotorOutput = 0.4; - public static double kDriveTurnBasicSingleMotorOutput = 0.15; - public static double kElevatorWheelDiameterInches = 1; - // Encoders - public static int kDriveEncoderTicksPerRev = 4096; - public static double kDriveEncoderTicksPerInch = (double)kDriveEncoderTicksPerRev / (kDriveWheelDiameterInches * Math.PI); - - // Elevator - public static int kArmEncoderTickesPerRev = 256; - public static double kArmInchesOfTravelPerRev = 3.75; - public static double kArmEncoderTicksPerInch = 126.36; - public static double kArmBasicPercentOutputUp = -0.85; - public static double kArmBasicPercentOutputDown =.7; - - // CONTROL LOOP GAINS - - // PID gains for drive velocity loop (LOW GEAR) - // Units: error is 4096 counts/rev. Max output is +/- 1023 units. - public static double kDriveVelocityKp = 1.0; - public static double kDriveVelocityKi = 0.0; - public static double kDriveVelocityKd = 6.0; - public static double kDriveVelocityKf = 0.5; - public static int kDriveVelocityIZone = 0; - public static double kDriveVelocityRampRate = 0.0; - public static int kDriveVelocityAllowableError = 0; - - // PID gains for drive base lock loop - // Units: error is 4096 counts/rev. Max output is +/- 1023 units. - public static double kDriveBaseLockKp = 0.5; - public static double kDriveBaseLockKi = 0; - public static double kDriveBaseLockKd = 0; - public static double kDriveBaseLockKf = 0; - public static int kDriveBaseLockIZone = 0; - public static double kDriveBaseLockRampRate = 0; - public static int kDriveBaseLockAllowableError = 10; - - // PID gains for constant heading velocity control - // Units: Error is degrees. Output is inches/second difference to - // left/right. - public static double kDriveHeadingVelocityKp = 4.0; // 6.0; - public static double kDriveHeadingVelocityKi = 0.0; - public static double kDriveHeadingVelocityKd = 50.0; - - // Path following constants - public static double kPathFollowingLookahead = 24.0; // inches - public static double kPathFollowingMaxVel = 120.0; // inches/sec - public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2 - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java deleted file mode 100644 index 91b4cae..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java +++ /dev/null @@ -1,22 +0,0 @@ -package org.usfirst.frc4388.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/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java deleted file mode 100644 index 4fc1133..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ /dev/null @@ -1,101 +0,0 @@ - - - -package org.usfirst.frc4388.robot; - -import buttons.XBoxTriggerButton; -import org.usfirst.frc4388.controller.IHandController; -import org.usfirst.frc4388.controller.XboxController; -import org.usfirst.frc4388.robot.commands.*; - - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.buttons.*; -import org.usfirst.frc4388.robot.subsystems.*; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; -import org.usfirst.frc4388.robot.subsystems.Drive; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.buttons.Button; -import edu.wpi.first.wpilibj.buttons.InternalButton; -import edu.wpi.first.wpilibj.buttons.JoystickButton; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import jaci.pathfinder.Pathfinder; - - - -public class OI -{ - private static OI instance; - - private XboxController m_driverXbox; - private XboxController m_operatorXbox; - - private OI() - { - try - { - // Driver controller - m_driverXbox = new XboxController(RobotMap.DRIVER_JOYSTICK_1_USB_ID); - m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID); - - /* XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER); - CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - CarriageIntake.whenReleased(new IntakeSetSpeed(0.0)); - - XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER); - CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); - CarriageEject.whenReleased(new IntakeSetSpeed(0.0)); - */ - JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); - climbUp.whenPressed(new InitiateClimber(true)); - climbUp.whenReleased(new InitiateClimber(false)); - - JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); - shiftUp.whenPressed(new DriveSpeedShift(true)); - // shiftUp.whenPressed(new LEDIndicators(true)); - - JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - shiftDown.whenPressed(new DriveSpeedShift(false)); - // shiftDown.whenPressed(new LEDIndicators(false)); - - - //Operator Xbox -/* - JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); - openIntake.whenPressed(new IntakePosition(true)); - - JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); - CloseIntake.whenPressed(new IntakePosition(false)); - - SmartDashboard.putData("PRE GAME!!!!", new PreGame()); - */ - } catch (Exception e) { - System.err.println("An error occurred in the OI constructor"); - } - } - - public static OI getInstance() { - if(instance == null) { - instance = new OI(); - } - return instance; - } - - public IHandController getDriverController() { - return m_driverXbox; - } - - public IHandController getOperatorController() - { - return m_operatorXbox; - } - - public Joystick getOperatorJoystick() - { - return m_operatorXbox.getJoyStick(); - } - } - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java deleted file mode 100644 index 5d66455..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ /dev/null @@ -1,161 +0,0 @@ - -package org.usfirst.frc4388.robot; - -import edu.wpi.first.wpilibj.IterativeRobot; -import edu.wpi.first.wpilibj.CameraServer; -import edu.wpi.first.wpilibj.DriverStation; -//import edu.wpi.first.wpilibj.PowerDistributionPanel; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.command.Scheduler; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -//import org.usfirst.frc4388.controller.Robot.OperationMode; -import org.usfirst.frc4388.robot.commands.*; - -import org.usfirst.frc4388.robot.OI; -import org.usfirst.frc4388.robot.subsystems.*; -import org.usfirst.frc4388.utility.ControlLooper; -import org.usfirst.frc4388.robot.subsystems.Drive; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;; - -public class Robot extends IterativeRobot -{ - - public static OI oi; - - public static final Drive drive = new Drive(); - - - - public static final Arm arm = new Arm(); - public static final Climber climber = new Climber(); - public static final Pnumatics pnumatics = new Pnumatics(); - public static final long periodMS = 10; - public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS); - - public static enum OperationMode { TEST, COMPETITION }; - public static OperationMode operationMode = OperationMode.TEST; - - private SendableChooser operationModeChooser; - private SendableChooser RRautonTaskChooser; - private SendableChooser RLautonTaskChooser; - private SendableChooser LRautonTaskChooser; - private SendableChooser LLautonTaskChooser; - - private Command RRautonomousCommand; - private Command RLautonomousCommand; - private Command LRautonomousCommand; - private Command LLautonomousCommand; - - public void robotInit() - { - //Printing game data to riolog - String gameData = DriverStation.getInstance().getGameSpecificMessage(); - System.out.println(gameData); - CameraServer.getInstance().startAutomaticCapture(); - CameraServer.getInstance().putVideo("res", 300, 220); - - try { - oi = OI.getInstance(); - - controlLoop.addLoopable(drive); - controlLoop.addLoopable(arm); - - - operationModeChooser = new SendableChooser(); - operationModeChooser.addDefault("Test", OperationMode.TEST); - operationModeChooser.addObject("Competition", OperationMode.COMPETITION); - SmartDashboard.putData("Operation Mode", operationModeChooser); - - - - - - - - - //ledLights.setAllLightsOn(false); - } catch (Exception e) { - System.err.println("An error occurred in robotInit()"); - } - } - - public void disabledInit(){ - - } - - public void disabledPeriodic() { - Scheduler.getInstance().run(); - updateStatus(); - } - - public void autonomousInit() { - updateChoosers(); - - controlLoop.start(); - drive.endGyroCalibration(); - drive.resetEncoders(); - drive.resetGyro(); - drive.setIsRed(getAlliance().equals(Alliance.Red)); - - } - - - - public void autonomousPeriodic() { - Scheduler.getInstance().run(); - updateStatus(); - } - - public void teleopInit() { - if (RRautonomousCommand != null) RRautonomousCommand.cancel(); - if (RLautonomousCommand != null) RLautonomousCommand.cancel(); - if (LRautonomousCommand != null) LRautonomousCommand.cancel(); - if (LLautonomousCommand != null) LLautonomousCommand.cancel(); - drive.setToBrakeOnNeutral(false); - updateChoosers(); - controlLoop.start(); - drive.resetEncoders(); - drive.endGyroCalibration(); - - updateStatus(); - } - - - public void teleopPeriodic() - { - Scheduler.getInstance().run(); - updateStatus(); - } - - public void testPeriodic() { - LiveWindow.run(); - updateStatus(); - } - - private void updateChoosers() { - operationMode = (OperationMode)operationModeChooser.getSelected(); - RRautonomousCommand = (Command)RRautonTaskChooser.getSelected(); - RLautonomousCommand = (Command)RLautonTaskChooser.getSelected(); - LRautonomousCommand = (Command)LRautonTaskChooser.getSelected(); - LLautonomousCommand = (Command)LLautonTaskChooser.getSelected(); - } - - public Alliance getAlliance() - { - return m_ds.getAlliance(); - } - - public void updateStatus() - { - drive.updateStatus(operationMode); - arm.updateStatus(operationMode); - } - -} - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java deleted file mode 100644 index 0ef7d9e..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ /dev/null @@ -1,36 +0,0 @@ - -package org.usfirst.frc4388.robot; - - -public class RobotMap { - // USB Port IDs - public static final int DRIVER_JOYSTICK_1_USB_ID = 0; - public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; - - - // MOTORS - - public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2; - public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3; - - - public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; - public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; - - //carriage motors - public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8; - public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9; - - //Arm Motors - public static final int ARM_MOTOR1_ID = 6; - public static final int ARM_MOTOR2_ID = 7; - public static final int CLIMBER_CAN_ID = 10; - - - // Pneumatics - public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0; - public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; - public static final int OPEN_INTAKE_PCM_ID = 4; - public static final int CLOSE_INTAKE_PCM_ID = 5; - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java deleted file mode 100644 index 8d724c4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java +++ /dev/null @@ -1,99 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class ArmBasic extends Command { - private double m_targetHeightInchesAboveFloor; - private double m_speed; - private boolean m_goingUp; - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - public static boolean isfinishedElevatorBasic; - - public ArmBasic(double targetHeightInchesAboveFloor) { - requires(Robot.arm); - m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; - } - - // Called just before this Command runs the first time - protected void initialize() - { - Robot.arm.setControlMode(DriveControlMode.RAW); - - double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); - // start out at half speed, as crude way to reduce slippage - m_goingUp = (m_targetHeightInchesAboveFloor > currentHeight); -System.out.println("initialize(): cur=" + currentHeight + " , target=" + m_targetHeightInchesAboveFloor + " , going " + (m_goingUp ? "UP" : "DOWN")); - if (m_goingUp) { - m_speed = Constants.kArmBasicPercentOutputUp; - } - else { - m_speed = Constants.kArmBasicPercentOutputDown; - } - m_commandInitTimestamp = Timer.getFPGATimestamp(); - - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - Robot.arm.rawSetOutput(m_speed); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double currentHeight = Robot.arm.getArmHeightInchesAboveFloor(); - double remaining = (m_targetHeightInchesAboveFloor - currentHeight) * (m_goingUp ? 1.0 : -1.0); -System.out.println("cur=" + currentHeight + " , remaining=" + remaining + " , target=" + m_targetHeightInchesAboveFloor); - if (remaining < 0.0) { - finished = true; - - } - /*} else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - }*/ - - - if (!finished) { - SmartDashboard.putNumber("EB Dist", currentHeight); - } else { - SmartDashboard.putNumber("EB finDist", currentHeight); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("EB Runtime", currentTimestamp - m_commandInitTimestamp); - - isfinishedElevatorBasic = isFinished(); - - Robot.arm.rawSetOutput(0.0); - - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java deleted file mode 100644 index 21e328a..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import org.usfirst.frc4388.robot.subsystems.*; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class ArmSetSpeed extends Command { - - private double RaiseSpeed; - - // Constructor with speed - public ArmSetSpeed(double RaiseSpeed) { - this.RaiseSpeed = RaiseSpeed; - // requires(Robot.elevatorAuton); - } - - // Called just before this Command runs the first time - protected void initialize() { - //Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return true; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java deleted file mode 100644 index 5c25540..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveAbsoluteTurnMP extends Command -{ - private double absoluteTurnAngleDeg, maxTurnRateDegPerSec; - private MPSoftwareTurnType turnType; - - public DriveAbsoluteTurnMP(double absoluteTurnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) { - requires(Robot.drive); - this.absoluteTurnAngleDeg = absoluteTurnAngleDeg; - this.maxTurnRateDegPerSec = maxTurnRateDegPerSec; - this.turnType = turnType; - } - - protected void initialize() { -// if (Robot.drive.isRed() == false) { -// absoluteTurnAngleDeg = absoluteTurnAngleDeg * -1; -// } - Robot.drive.setAbsoluteTurnMP(absoluteTurnAngleDeg, maxTurnRateDegPerSec, turnType); - } - - protected void execute() { - } - - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java deleted file mode 100644 index 9d2d3e4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveGyroReset extends Command -{ - public DriveGyroReset() { - requires(Robot.drive); - } - - @Override - protected void initialize() { - Robot.drive.resetGyro(); - Robot.drive.resetEncoders(); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - - } - - @Override - protected void interrupted() { - - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java deleted file mode 100644 index 0bc270b..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveRelativeTurnPID extends Command -{ - private double relativeTurnAngleDeg; - private MPSoftwareTurnType turnType; - - public DriveRelativeTurnPID(double relativeTurnAngleDeg, MPSoftwareTurnType turnType) { - requires(Robot.drive); - this.relativeTurnAngleDeg = relativeTurnAngleDeg; - this.turnType = turnType; - } - - protected void initialize() { - Robot.drive.setRelativeTurnPID(relativeTurnAngleDeg, 0.3, 0.1, turnType); - } - - protected void execute() { - } - - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java deleted file mode 100644 index d3e8293..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveSpeedShift extends Command -{ - public boolean speed; - - public DriveSpeedShift(boolean speed) { - this.speed=speed; - requires(Robot.pnumatics); - } - - @Override - protected void initialize() { - Robot.pnumatics.setShiftState(speed); - } - - @Override - protected void execute() { - - } - - @Override - protected boolean isFinished() { - return true; - } - - @Override - protected void end() { - - } - - @Override - protected void interrupted() { - - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java deleted file mode 100644 index a477dc7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java +++ /dev/null @@ -1,103 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class DriveStraightBasic extends Command { - private double m_targetInches; - private double m_maxVelocityInchesPerSec; - private double m_speed; - private boolean m_goingBackwards; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - - public DriveStraightBasic(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_targetInches = targetInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - m_goingBackwards = (m_targetInches < 0.0); - } - - protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { - double sign = (backwards ? -1.0 : 1.0); - // Keep velocity above stiction limit (else bot will freeze and command will not complete) - double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); - // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) - return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.drive.resetGyro(); - Robot.drive.resetEncoders(); - Robot.drive.setControlMode(DriveControlMode.RAW); - // start out at half speed, as crude way to reduce slippage - m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); - m_commandInitTimestamp = Timer.getFPGATimestamp(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - double steer = 0.0; - if (m_useGyroLock) { - steer = - Robot.drive.getGyroAngleDeg() / 25.0; //TODO: tune - } - Robot.drive.rawMoveSteer(m_speed, steer); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double velocity = m_maxVelocityInchesPerSec; - double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2; - double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); - if (remaining < 0.0) { - finished = true; - } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - } - if (!finished) { - m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); - } else { - SmartDashboard.putNumber("DSB finDist", position); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); - Robot.drive.rawMoveSteer(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java deleted file mode 100644 index 21fede7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java +++ /dev/null @@ -1,115 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -public class DriveStraightBasicAbs extends Command { - private double m_targetInches; - private double m_maxVelocityInchesPerSec; - private double m_speed; - private boolean m_goingBackwards; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - private double m_targetGyroAngleDeg; // what we want the gyro to read as we're driving straight - private double m_commandInitTimestamp; - private double m_lastCommandExecuteTimestamp = 0.0; - private double m_lastCommandExecutePeriod = 0.0; - - public DriveStraightBasicAbs(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_targetInches = targetInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - m_goingBackwards = (m_targetInches < 0.0); - } - - protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { - double sign = (backwards ? -1.0 : 1.0); - // Keep velocity above stiction limit (else bot will freeze and command will not complete) - double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); - // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) - return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; - } - - // Called just before this Command runs the first time - protected void initialize() { - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - if (m_useAbsolute) { - m_targetGyroAngleDeg = m_desiredAbsoluteAngle; - } else { - m_targetGyroAngleDeg = currentAngleDeg; - } - Robot.drive.resetEncoders(); - Robot.drive.setControlMode(DriveControlMode.RAW); - // start out at half speed, as crude way to reduce slippage - m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); - m_commandInitTimestamp = Timer.getFPGATimestamp(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time - m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; - } - m_lastCommandExecuteTimestamp = currentTimestamp; - double steer = 0.0; - if (m_useGyroLock) { - double yawError = Robot.drive.getGyroAngleDeg() - m_targetGyroAngleDeg; - steer = -yawError / Constants.kDriveStraightBasicYawErrorDivisor; - if (steer < -Constants.kDriveStraightBasicMaxSteerMagnitude) { - steer = -Constants.kDriveStraightBasicMaxSteerMagnitude; - } else if (Constants.kDriveStraightBasicMaxSteerMagnitude < steer) { - steer = Constants.kDriveStraightBasicMaxSteerMagnitude; - } - } - Robot.drive.rawMoveSteer(m_speed, steer); - //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - boolean finished=false; - double velocity = m_maxVelocityInchesPerSec; - double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2; - double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); - if (remaining < 0.0) { - finished = true; - } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms - velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed - } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms - velocity = m_maxVelocityInchesPerSec / 2.0; // half speed - } - if (!finished) { - m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); - } else { - SmartDashboard.putNumber("DSB finDist", position); - } - return finished; - } - - // Called once after isFinished returns true - protected void end() { - double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); - Robot.drive.rawMoveSteer(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java deleted file mode 100644 index e205818..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java +++ /dev/null @@ -1,60 +0,0 @@ -// RobotBuilder Version: 2.0 -// -// This file was generated by RobotBuilder. It contains sections of -// code that are automatically generated and assigned by robotbuilder. -// These sections will be updated in the future when you export to -// Java from RobotBuilder. Do not put any code or make any change in -// the blocks indicating autogenerated code or it will be lost on an -// update. Deleting the comments indicating the section will prevent -// it from being updated in the future. - - -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - - -public class DriveStraightMP extends Command { - private double m_distanceInches; - private double m_maxVelocityInchesPerSec; - private boolean m_useGyroLock; - private boolean m_useAbsolute; - private double m_desiredAbsoluteAngle; - - public DriveStraightMP(double distanceInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - requires(Robot.drive); - m_distanceInches = distanceInches; - m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; - m_useGyroLock = useGyroLock; - m_useAbsolute = useAbsolute; - m_desiredAbsoluteAngle = desiredAbsoluteAngle; - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.drive.setStraightMP(m_distanceInches, m_maxVelocityInchesPerSec, m_useGyroLock, m_useAbsolute, m_desiredAbsoluteAngle); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return Robot.drive.isFinished(); - } - - // Called once after isFinished returns true - protected void end() { - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - end(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java deleted file mode 100644 index 82223d9..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java +++ /dev/null @@ -1,151 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.BHRMathUtils; -import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.command.Command; - -public class DriveTurnBasic extends Command -{ - private boolean m_useAbsolute; // if true, we don't reset the gyro before starting the turn - private double m_turnAngleDeg; // either absolute or relative to current heading, depending on useAbsolute - private double m_maxTurnRateDegPerSec; - private MPSoftwareTurnType m_turnType; - private boolean m_turningLeft; - private double m_targetGyroAngleDeg; // what we want the gyro to read when we're done - - public DriveTurnBasic(boolean useAbsolute, double turnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) { - requires(Robot.drive); - m_useAbsolute = useAbsolute; - m_turnAngleDeg = turnAngleDeg; - m_maxTurnRateDegPerSec = maxTurnRateDegPerSec; - m_turnType = turnType; - } - - protected void initialize() { - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - if (m_useAbsolute) { - m_targetGyroAngleDeg = m_turnAngleDeg; - } else { - m_targetGyroAngleDeg = currentAngleDeg + m_turnAngleDeg; - } - if ((m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.RIGHT_ARC)) { - m_turningLeft = true; // gyro angle will be DEcreasing, so make sure we aim for an angle in that direction - while (m_targetGyroAngleDeg >= currentAngleDeg) { - m_targetGyroAngleDeg = m_targetGyroAngleDeg - 360.0; - } - } else if ((m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.LEFT_ARC)) { - m_turningLeft = false; // gyro angle will be INcreasing, so make sure we aim for an angle in that direction - while (m_targetGyroAngleDeg <= currentAngleDeg) { - m_targetGyroAngleDeg = m_targetGyroAngleDeg + 360.0; - } - } else { // MPSoftwareTurnType.TANK -- need to determine based on values passed - if (m_useAbsolute) { // this is the only case where we have to DECIDE which direction to turn - m_turnAngleDeg = BHRMathUtils.normalizeAngle0To360(m_turnAngleDeg); - m_targetGyroAngleDeg = BHRMathUtils.adjustAccumAngleToDesired(currentAngleDeg, m_turnAngleDeg); - m_turningLeft = (m_targetGyroAngleDeg < currentAngleDeg); - } else { - m_turningLeft = (m_turnAngleDeg < 0); - } - } - System.out.println("Turning " + (m_turningLeft?"left":"right") + " from " + currentAngleDeg + " to " + m_targetGyroAngleDeg + " degrees"); - Robot.drive.setControlMode(DriveControlMode.RAW); - Robot.drive.resetEncoders(); - } - - protected void execute() { - double output = Constants.kDriveTurnBasicSingleMotorOutput; - - if (m_turnType == MPSoftwareTurnType.TANK) { - output = Constants.kDriveTurnBasicTankMotorOutput; - if (m_turningLeft) { - Robot.drive.rawDriveLeftRight(-output, -output); // left backward, right forward - } else { - Robot.drive.rawDriveLeftRight(output, output); // left forward, right backward - } -// for (CANTalonEncoder motorController : motorControllers) { -// //motorController.set(output); -// motorController.set(ControlMode.PercentOutput, output); -// } - } - else if (m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - Robot.drive.rawDriveLeftRight(2.0 * output, 0.0); // left forward double speed -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(0); -// motorController.set(ControlMode.PercentOutput, 0); -// } -// else { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - Robot.drive.rawDriveLeftRight(0.0, -2.0 * output); // right forward double speed -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// else { -// //motorController.set(0); -// motorController.set(ControlMode.PercentOutput, 0); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.LEFT_ARC) { - Robot.drive.rawDriveLeftRight(2.0 * output, -output); // left fwd 2x, right fwd 1x -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(1.0 * output); -// motorController.set(ControlMode.PercentOutput, 1.0 * output); -// } -// else { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// } - } - else if (m_turnType == MPSoftwareTurnType.RIGHT_ARC) { - Robot.drive.rawDriveLeftRight(output, -2.0 * output); // left fwd 1x, right fwd 2x -// for (CANTalonEncoder motorController : motorControllers) { -// if (motorController.isRight()) { -// //motorController.set(2.0 * output); -// motorController.set(ControlMode.PercentOutput, 2.0 * output); -// } -// else { -// //motorController.set(1.0 * output); -// motorController.set(ControlMode.PercentOutput, 1.0 * output); -// } -// } - } - } - - protected boolean isFinished() { - boolean finished; - double currentAngleDeg = Robot.drive.getGyroAngleDeg(); - - if (m_turningLeft) { - finished = currentAngleDeg <= m_targetGyroAngleDeg; - } else { - finished = currentAngleDeg >= m_targetGyroAngleDeg; - } - return finished; - } - - protected void end() { - Robot.drive.rawDriveLeftRight(0.0, 0.0); - Robot.drive.setControlMode(DriveControlMode.JOYSTICK); - } - - protected void interrupted() { - end(); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java deleted file mode 100644 index 8a6e37d..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.usfirst.frc4388.robot.commands; - -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -public class InitiateClimber extends Command -{ - boolean climb; - - public InitiateClimber(boolean climb) { - this.climb=climb; - requires(Robot.climber); - } - - @Override - protected void initialize() { - Robot.climber.setClimbSpeed(climb); - } - - @Override - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return true; - } - - // Called once after isFinished returns true - protected void end() { - } - - @Override - protected void interrupted() { - - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java deleted file mode 100644 index d70b56d..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ /dev/null @@ -1,424 +0,0 @@ -package org.usfirst.frc4388.robot.subsystems; - -import java.util.ArrayList; - -import org.usfirst.frc4388.controller.XboxController; -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.robot.commands.*; -import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; -import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.ControlLoopable; -import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.SoftwarePIDPositionController; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; -import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.SensorCollection; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -public class Arm extends Subsystem implements ControlLoopable -{ - //PID encoder and motor - private CANTalonEncoder armMotor; - //private WPI_TalonSRX ArmLeft; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerMaxScale; - //private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPMaxScale; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerLowScale; - //private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPLowScale; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerSwitch; - //private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPSwitch; - - //PID controller Max Scale - private SoftwarePIDPositionController pidPositionControllerLowest; - //private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0); - private PIDParams PositionPLowest; - - //PID target - private double targetPPosition; - - //Encoder ticks to inches for encoders - public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch; - - //control mode for joystick control - private DriveControlMode controlMode = DriveControlMode.JOYSTICK; - - private double periodMs; - - //Non Linear Joystick - public static final double STICK_DEADBAND = 0.02; - public static final double MOVE_NON_LINEARITY = 1.0; - private int moveNonLinear = 0; - private double moveScale = 1.0; - private double moveTrim = 0.0; - - private boolean isFinished; - private DifferentialDrive m_drive; - - //private LimitSwitchSource limitSwitch = new DigitalInput(1); - LimitSwitchSource limitSwitchSource; - - public boolean pressed; - SensorCollection isPressed; - - public boolean armControlMode = false; - // Motor controllers - //private ArrayList motorControllers = new ArrayList(); - - public Arm() - { - try - { - //PID Arm encoder and talon - armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); - - //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); - - //ArmMotor.setInverted(false); - - //Setting left Arm motor as follower - //ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID()); - //ArmLeft.setInverted(false); - //ArmLeft.setNeutralMode(NeutralMode.Brake); - armMotor.setNeutralMode(NeutralMode.Brake); - armMotor.setSensorPhase(true); - //Limit Switch Left - //ArmLeft.overrideLimitSwitchesEnable(true); - //ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - - //Limit Switch Right - //ArmMotor.overrideLimitSwitchesEnable(true); - //ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - - - //Change This boi - - // ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here - //ArmMotor.configReverseSoftLimitThreshold(5, 0); - //ArmMotor.configForwardSoftLimitEnable(true, 0); - //ArmMotor.configReverseSoftLimitEnable(true, 0); - - //sos - //ArmMotor.enableLimitSwitch(true, true); - - - - - - } - catch(Exception e) - { - System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); - } - } - - private double nonlinearStickCalcPositive(double move, double moveNonLinearity) - { - return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity); - } - - private double nonlinearStickCalcNegative(double move, double moveNonLinearity) - { - return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity); - } - - private boolean inDeadZone(double input) - { - boolean inDeadZone; - if (Math.abs(input) < STICK_DEADBAND) - { - inDeadZone = true; - } - else - { - inDeadZone = false; - } - - return inDeadZone; - } - - private double limitValue(double value) - { - if (value > 1.0) - { - value = 1.0; - } - else if (value < -1.0) - { - value = -1.0; - } - return value; - } - - public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity) - { - if (inDeadZone(move)) - { - return 0; - } - - move += trim; - move *= scale; - move = limitValue(move); - - int iterations = Math.abs(nonLinearFactor); - for (int i = 0; i < iterations; i++) - { - if (nonLinearFactor > 0) - { - move = nonlinearStickCalcPositive(move, wheelNonLinearity); - } - else - { - move = nonlinearStickCalcNegative(move, wheelNonLinearity); - } - } - return move; - } - - public void setArmMode() - { - setControlMode(DriveControlMode.JOYSTICK); - } - - public void resetArmEncoder() - { - armMotor.setSelectedSensorPosition(0, 0, 0); - } - - public void moveArmXbox() - { - double moveArmInput; - double armSafeZone = 15; - - double armPos = getEncoderArmPosition(); - - moveArmInput = Robot.oi.getOperatorController().getLeftYAxis(); - - //double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY); - - boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); - boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); - - if(armTuningPressed == true) - { - armMotor.set(moveArmInput * 0.5); - } - else if(armTuningPressed == false) - { - armMotor.set(moveArmInput); - } - } - - -// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range - - - //PID encoder position - public double getEncoderArmPosition() - { - return armMotor.getPositionWorld(); - } - - public double getArmHeightInchesAboveFloor() - { - return armMotor.getPositionWorld(); - } - - public synchronized void setControlMode(DriveControlMode controlMode) - { - this.controlMode = controlMode; - - isFinished = false; - } - /* - public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); - } - - public void setArmPIDLowScale(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); - } - - public void setArmPIDSwitch(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); - } - - public void setArmPIDLowest(double ArmPosition, double maxError, double minError) - { - double ArmTargetPos = 0; - this.targetPPosition = ArmTargetPos; - pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); - Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); - } - */ - public void rawSetOutput(double output){ - armMotor.set(/*ControlMode.PercentOutput,*/ output); - } - - public void holdInPos() - { - armMotor.set(-0.43 * 0.2); - } - - public void stopMotors() - { - armMotor.set(0); - } - - public void isSwitchPressed() - { - pressed = false; - isPressed = armMotor.getSensorCollection(); - - if(isPressed.isFwdLimitSwitchClosed() == true) - { - if (controlMode == DriveControlMode.JOYSTICK) { - Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS); - } - pressed = true; - } - else - { - if (controlMode == DriveControlMode.STOP_MOTORS){ - { - Robot.arm.setControlMode(DriveControlMode.JOYSTICK); - } - - pressed = false; - } - } - - } - - //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; - - - - - - @Override - public void controlLoopUpdate() - { - if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) - { - moveArmXbox(); - } - else if (!isFinished) - { - //PID control mode - if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE) - { - isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE) - { - isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH) - { - isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition()); - } - else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST) - { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); - } - /* - else if(controlMode == DriveControlMode.RAW) - { - isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); - } - */ - } - } - - @Override - public void initDefaultCommand() - { - } - - @Override - public void periodic() - { - // isSwitchPressed(); - } - - @Override - public void setPeriodMs(long periodMs) - { - //PID controller - pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor); - pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor); - pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor); - pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor); - - this.periodMs = periodMs; - } - - public synchronized boolean isFinished() - { - return isFinished; - } - - public double getPeriodMs() - { - return periodMs; - } - - public void updateStatus(Robot.OperationMode operationMode) - { - if (operationMode == Robot.OperationMode.TEST) - { - try - { - SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); - //SmartDashboard.putData(pressed); - } - catch (Exception e) - { - System.err.println("Drivetrain update status error"); - } - } - } - - -} - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java deleted file mode 100644 index 3cacf76..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java +++ /dev/null @@ -1,68 +0,0 @@ - - - - -package org.usfirst.frc4388.robot.subsystems; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.robot.commands.*; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.livewindow.LiveWindow; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - - -/** - * - */ -public class Climber extends Subsystem{ - - private WPI_TalonSRX Climber; - - public boolean on; - - public Climber(){ - - try{ - - Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); - - } catch (Exception e) { - - System.err.println("An error occurred in the climbing constructor"); - - } - } - - @Override - public void initDefaultCommand() { - - } - - - @Override - public void periodic() { - // Put code here to be run every loop - - } - - public void setClimbSpeed(boolean Climb) { - if (Climb==true) { - Climber.set(1.0); - } - if (Climb==false) { - Climber.set(0); - } -} - // Put methods for controlling this subsystem - // here. Call these from Commands. - { - // TODO Auto-generated method stub - - } - -} - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java deleted file mode 100644 index 3d29c64..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ /dev/null @@ -1,862 +0,0 @@ - -package org.usfirst.frc4388.robot.subsystems; - -import java.util.ArrayList; -import java.util.Set; - -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.OI; -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.utility.AdaptivePurePursuitController; -import org.usfirst.frc4388.utility.BHRMathUtils; -import org.usfirst.frc4388.utility.CANTalonEncoder; -import org.usfirst.frc4388.utility.ControlLoopable; -import org.usfirst.frc4388.utility.Kinematics; -import org.usfirst.frc4388.utility.MMTalonPIDController; -import org.usfirst.frc4388.utility.MPSoftwarePIDController; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; -import org.usfirst.frc4388.utility.MPTalonPIDController; -import org.usfirst.frc4388.utility.MPTalonPIDPathController; -import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController; -//import org.usfirst.frc4388.utility.SoftwarePIDPositionController; -import org.usfirst.frc4388.utility.MotionProfilePoint; -//import org.usfirst.frc4388.utility.MotionProfileBoxCar; -import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.Path; -import org.usfirst.frc4388.utility.PathGenerator; -import org.usfirst.frc4388.utility.RigidTransform2d; -import org.usfirst.frc4388.utility.Rotation2d; -import org.usfirst.frc4388.utility.SoftwarePIDController; -import org.usfirst.frc4388.utility.Translation2d; - -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -//import com.ctre.PigeonImu; -//import com.ctre.PigeonImu.CalibrationMode; -import com.ctre.phoenix.motorcontrol.NeutralMode; - -import com.kauailabs.navx.frc.AHRS; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -//import edu.wpi.first.wpilibj.DigitalInput; -//import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.SPI; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -//import edu.wpi.first.wpilibj.Solenoid; -//import edu.wpi.first.wpilibj.SpeedControllerGroup; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - - -/** - * - */ -public class Drive extends Subsystem implements ControlLoopable -{ - public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW}; - public static enum SpeedShiftState { HI, LO }; - public static enum ClimberState { DEPLOYED, RETRACTED }; - - public static final double TRACK_WIDTH_INCHES = Constants.kTrackWidthInches; - public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch; - - public static final double CLIMB_SPEED = 0.45; - - public static final double VOLTAGE_RAMP_RATE = 150; // Volts per second - public static final double OPEN_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full - public static final double CLOSED_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full - - // Motion profile max velocities and accel times - public static final double MAX_TURN_RATE_DEG_PER_SEC = 320; - public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC = 120; //72; - public static final double MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC = 200; - public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 320; - public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 400; - public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC = 270; - public static final double MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC = 400; - public static final double MP_GEAR_DEPLOY_VELOCITY_INCHES_PER_SEC = 25; - public static final double MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC = 80; - - - - - public static final double MP_STRAIGHT_T1 = 600; - public static final double MP_STRAIGHT_T2 = 300; - public static final double MP_TURN_T1 = 600; - public static final double MP_TURN_T2 = 300; - public static final double MP_MAX_TURN_T1 = 400; - public static final double MP_MAX_TURN_T2 = 200; - - // Motor controllers - private ArrayList motorControllers = new ArrayList(); - - private CANTalonEncoder leftDrive1; - private WPI_TalonSRX leftDrive2; -// private WPI_TalonSRX leftDrive3; - - private CANTalonEncoder rightDrive1; - private WPI_TalonSRX rightDrive2; -// private WPI_TalonSRX rightDrive3; - - private DifferentialDrive m_drive; - - //PID encoder and motor - private CANTalonEncoder elevatorRight; - private WPI_TalonSRX elevatorLeft; - - //private DigitalInput hopperSensorRed; - //private DigitalInput hopperSensorBlue; - - - - private double climbSpeed; - - private boolean isRed = true; - - private double periodMs; - private double lastControlLoopUpdatePeriod = 0.0; - private double lastControlLoopUpdateTimestamp = 0.0; - - // Pneumatics - //private Solenoid speedShift; - - // Input devices - public static final int DRIVER_INPUT_JOYSTICK_ARCADE = 0; - public static final int DRIVER_INPUT_JOYSTICK_TANK = 1; - public static final int DRIVER_INPUT_JOYSTICK_CHEESY = 2; - public static final int DRIVER_INPUT_XBOX_CHEESY = 3; - public static final int DRIVER_INPUT_XBOX_ARCADE_LEFT = 4; - public static final int DRIVER_INPUT_XBOX_ARCADE_RIGHT = 5; - public static final int DRIVER_INPUT_WHEEL = 6; - - public static final double STEER_NON_LINEARITY = 0.5; - public static final double MOVE_NON_LINEARITY = 1.0; - - public static final double STICK_DEADBAND = 0.02; - - private int m_moveNonLinear = 0; - private int m_steerNonLinear = -3; - - private double m_moveScale = 1.0; - private double m_steerScale = 1.0; - - private double m_moveInput = 0.0; - private double m_steerInput = 0.0; - - private double m_moveOutput = 0.0; - private double m_steerOutput = 0.0; - - private double m_moveTrim = 0.0; - private double m_steerTrim = 0.0; - - private boolean isFinished; - private DriveControlMode controlMode = DriveControlMode.JOYSTICK; - - private MPTalonPIDController mpStraightController; - private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7); // 4 colsons -// private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03); // 4 omni - private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0); - - private MMTalonPIDController mmStraightController; - private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24); - - private MPSoftwarePIDController mpTurnController; // p i d a v g izone - private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100); // 4 colson wheels -// private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100); // 4 omni - - private SoftwarePIDController pidTurnController; - //private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008 - private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.0, 0.0, 0, 0, 0.0, 100); //i=0.0008 - private double targetPIDAngle; - - private MPTalonPIDPathController mpPathController; - private PIDParams mpPathPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.28, 100); // 4 omni g=.3 - - //PID target - private double targetPIDPosition; - - private MPTalonPIDPathVelocityController mpPathVelocityController; - private PIDParams mpPathVelocityPIDParams = new PIDParams(0.5, 0.001, 5, 0.44); - - private AdaptivePurePursuitController adaptivePursuitController; - private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44); - - private RigidTransform2d zeroPose = new RigidTransform2d(new Translation2d(0,0), Rotation2d.fromDegrees(0)); - private RigidTransform2d currentPose = zeroPose; - private RigidTransform2d lastPose = zeroPose; - private double left_encoder_prev_distance_ = 0; - private double right_encoder_prev_distance_ = 0; - - //private PigeonImu gyroPigeon; - //private double[] yprPigeon = new double[3]; - private AHRS gyroNavX; - private boolean useGyroLock; - private double gyroLockAngleDeg; - //private double kPGyro = 0.04; - private double kPGyro = 0.0625; - private boolean isCalibrating = false; - private double gyroOffsetDeg = 0; - - public Drive() { - try { - leftDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); - leftDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID); -// leftDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID); - - rightDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder); - rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); -// rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); - - //gyroPigeon = new PigeonImu(leftDrive2); - gyroNavX = new AHRS(SPI.Port.kMXP); - - //hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID); - //hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID); - - //leftDrive1.clearStickyFaults(); - //leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); - //leftDrive1.setNominalClosedLoopVoltage(12.0); - leftDrive1.clearStickyFaults(0); - leftDrive1.setInverted(false);//false on comp 2108, false on microbot - leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot - leftDrive1.setSafetyEnabled(false); - //leftDrive1.setCurrentLimit(15); - //leftDrive1.enableCurrentLimit(true); - leftDrive1.setNeutralMode(NeutralMode.Brake); - leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); - leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); - leftDrive1.configNominalOutputForward(+1.0f, 0); - leftDrive1.configNominalOutputReverse(-1.0f, 0); - - -// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { -// Driver.reportError("Could not detect left drive encoder encoder!", false); -// } - - - leftDrive2.setInverted(false);//false on comp 2108, false on microbot - leftDrive2.setSafetyEnabled(false); - leftDrive2.setNeutralMode(NeutralMode.Brake); - leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); - - - - //rightDrive1.clearStickyFaults(); - //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); - //rightDrive1.setNominalClosedLoopVoltage(12.0); - rightDrive1.clearStickyFaults(0); - rightDrive1.setInverted(true);//true on comp 2108, false on microbot - rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot - rightDrive1.setSafetyEnabled(false); - rightDrive1.setNeutralMode(NeutralMode.Brake); - rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); - rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); - rightDrive1.configNominalOutputForward(+1.0f, 0); - rightDrive1.configNominalOutputReverse(-1.0f, 0); -// if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { -// DriverStation.reportError("Could not detect right drive encoder encoder!", false); -// } - - - rightDrive2.setInverted(true);//True on comp 2108, false on microbot - rightDrive2.setSafetyEnabled(false); - rightDrive2.setNeutralMode(NeutralMode.Brake); - rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); - - - - - motorControllers.add(leftDrive1); - motorControllers.add(rightDrive1); - - //m_drive = new RobotDrive(leftDrive1, rightDrive1); - //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); - //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true); - //m_drive.setSafetyEnabled(false); - m_drive = new DifferentialDrive(leftDrive1, rightDrive1); - //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearLeft, true); //TODO URGENT: verify - //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify - m_drive.setSafetyEnabled(false); - - //speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID); - } - catch (Exception e) { - System.err.println("An error occurred in the DriveTrain constructor"); - } - } - - public void setToBrakeOnNeutral(boolean brakeVsCoast) { - if (brakeVsCoast) { - leftDrive1.setNeutralMode(NeutralMode.Brake); - leftDrive2.setNeutralMode(NeutralMode.Brake); - rightDrive1.setNeutralMode(NeutralMode.Brake); - rightDrive2.setNeutralMode(NeutralMode.Brake); - } else { - leftDrive1.setNeutralMode(NeutralMode.Coast); - leftDrive2.setNeutralMode(NeutralMode.Coast); - rightDrive1.setNeutralMode(NeutralMode.Coast); - rightDrive2.setNeutralMode(NeutralMode.Coast); - } - } - - @Override - public void initDefaultCommand() { - } - - public double getGyroAngleDeg() { - //return getGyroPigeonAngleDeg(); - return getGyroNavXAngleDeg(); - } - - //public double getGyroPigeonAngleDeg() { - // gyroPigeon.GetYawPitchRoll(yprPigeon); - // return -yprPigeon[0] + gyroOffsetDeg; - //} - - public double getGyroNavXAngleDeg() { - return gyroNavX.getAngle() + gyroOffsetDeg; - } - - public void resetGyro() { - //gyroPigeon.SetYaw(0); - gyroNavX.zeroYaw(); - } - - public void resetEncoders() { - mpStraightController.resetZeroPosition(); - } - - public void calibrateGyro() { - //gyroPigeon.EnterCalibrationMode(CalibrationMode.Temperature); - } - - public void endGyroCalibration() { - if (isCalibrating == true) { - isCalibrating = false; - } - } - - public void setGyroOffset(double offsetDeg) { - gyroOffsetDeg = offsetDeg; - } - - //public boolean isHopperSensorRedOn() { - // return hopperSensorRed.get(); - //} - - //public boolean isHopperSensorBlueOn() { - // return hopperSensorBlue.get(); - //} - - //public boolean isHopperSensorOn() { - // if (isRed() == true) { - // return isHopperSensorRedOn(); - // } - // else { - // return isHopperSensorBlueOn(); - // } - //} - - public void setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - mmStraightController.setPID(mmStraightPIDParams); - mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true); - setControlMode(DriveControlMode.MOTION_MAGIC); - } - - public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - mpStraightController.setPID(mpStraightPIDParams); - mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); - setControlMode(DriveControlMode.MP_STRAIGHT); - } - - //public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { - // double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); - // mpStraightController.setPID(mpStraightPIDParams); - // mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); - // setControlMode(DriveControlMode.MP_STRAIGHT); - //} - - public void setRelativeTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { - mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); - setControlMode(DriveControlMode.MP_TURN); - } - - //public void setRelativeTurnMPCached(String key, MPSoftwareTurnType turnType) { - // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); - // setControlMode(DriveControlMode.MP_TURN); - //} - - public void setRelativeMaxTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { - mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_MAX_TURN_T1, MP_MAX_TURN_T2, turnType, TRACK_WIDTH_INCHES); - setControlMode(DriveControlMode.MP_TURN); - } - - public void setAbsoluteTurnMP(double absoluteTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { - mpTurnController.setMPTurnTarget(getGyroAngleDeg(), BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), absoluteTurnAngleDeg), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); - setControlMode(DriveControlMode.MP_TURN); - } - - //public void setAbsoluteTurnMPCached(String key, MPSoftwareTurnType turnType) { - // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); - // setControlMode(DriveControlMode.MP_TURN); - //} - - public void setRelativeTurnPID(double relativeTurnAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) { - this.targetPIDAngle = relativeTurnAngleDeg + getGyroAngleDeg(); - pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType); - setControlMode(DriveControlMode.PID_TURN); - } - - public void setPathMP(PathGenerator path) { - mpPathController.setPID(mpPathPIDParams); - mpPathController.setMPPathTarget(path); - setControlMode(DriveControlMode.MP_PATH); - } - - public void setPathVelocityMP(PathGenerator path) { - mpPathVelocityController.setPID(mpPathPIDParams); - mpPathVelocityController.setMPPathTarget(path); - setControlMode(DriveControlMode.MP_PATH_VELOCITY); - } - - public void setPathAdaptivePursuit(Path path, boolean reversed) { - currentPose = zeroPose; - lastPose = zeroPose; - left_encoder_prev_distance_ = 0; - right_encoder_prev_distance_ = 0; - adaptivePursuitController.setPID(adaptivePursuitPIDParams); - adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); - setControlMode(DriveControlMode.ADAPTIVE_PURSUIT); - } - - public void setDriveHold(boolean status) { - if (status) { - setControlMode(DriveControlMode.HOLD); - } - else { - setControlMode(DriveControlMode.JOYSTICK); - } - } - - public void updatePose() { - double left_distance = leftDrive1.getPositionWorld(); - double right_distance = rightDrive1.getPositionWorld(); - Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg()); - lastPose = currentPose; - currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle); - left_encoder_prev_distance_ = left_distance; - right_encoder_prev_distance_ = right_distance; - } - - public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) { - return Kinematics.integrateForwardKinematics(lastPose, left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle); - } - - /** - * Path Markers are an optional functionality that name the various - * Waypoints in a Path with a String. This can make defining set locations - * much easier. - * - * @return Set of Strings with Path Markers that the robot has crossed. - */ - public synchronized Set getPathMarkersCrossed() { - if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) { - return null; - } else { - return adaptivePursuitController.getMarkersCrossed(); - } - } - - public synchronized void setControlMode(DriveControlMode controlMode) { - this.controlMode = controlMode; - if (controlMode == DriveControlMode.JOYSTICK) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.MANUAL) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.CLIMB) { - //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); - //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); - leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving - } - else if (controlMode == DriveControlMode.HOLD) { - mpStraightController.setPID(mpHoldPIDParams); - //leftDrive1.changeControlMode(TalonControlMode.Position); - //leftDrive1.setPosition(0); - //leftDrive1.set(0); - //rightDrive1.changeControlMode(TalonControlMode.Position); - //rightDrive1.setPosition(0); - //rightDrive1.set(0); - leftDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - leftDrive1.set(ControlMode.Position, 0); - rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - rightDrive1.set(ControlMode.Position, 0); - } - isFinished = false; - } - - public synchronized void controlLoopUpdate() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (lastControlLoopUpdateTimestamp > 0.001) { // ie, if this is NOT the first time - lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; - } - lastControlLoopUpdateTimestamp = currentTimestamp; - - // Do the update - if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) { - driveWithJoystick(); - } - else if (!isFinished) { - if (controlMode == DriveControlMode.MP_STRAIGHT) { - isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MP_TURN) { - isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.PID_TURN) { - isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MP_PATH) { - isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) { - isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.MOTION_MAGIC) { - isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) { - updatePose(); - isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); - } - } - } - - public void setSpeed(double speed) { - if (speed == 0) { - setControlMode(DriveControlMode.JOYSTICK); - } - else { - setControlMode(DriveControlMode.MANUAL); - rightDrive1.set(-speed); - leftDrive1.set(speed); - } - } - - public void setClimbSpeed(double climbSpeed) { - this.climbSpeed = climbSpeed; - if (climbSpeed == 0) { - setControlMode(DriveControlMode.JOYSTICK); - } - else { - setControlMode(DriveControlMode.CLIMB); - } - } - - public void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) { - if (snapToAbsolute0or180) { - gyroLockAngleDeg = BHRMathUtils.adjustAccumAngleToClosest180(getGyroAngleDeg()); - } - else { - gyroLockAngleDeg = getGyroAngleDeg(); - } - this.useGyroLock = useGyroLock; - } - - public void driveWithJoystick() { - if(m_drive == null) return; - // switch(m_controllerMode) { - // case CONTROLLER_JOYSTICK_ARCADE: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick1().getX(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_JOYSTICK_TANK: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick2().getY(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_steerInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_drive.tankDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_JOYSTICK_CHEESY: - // m_moveInput = OI.getInstance().getJoystick1().getY(); - // m_steerInput = OI.getInstance().getJoystick2().getX(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_XBOX_CHEESY: - // boolean turbo = OI.getInstance().getDriveTrainController() - // .getLeftJoystickButton(); - // boolean slow = OI.getInstance().getDriveTrainController() - // .getRightJoystickButton(); - // double speedToUseMove, speedToUseSteer; - // if (turbo && !slow) { - // speedToUseMove = m_moveScaleTurbo; - // speedToUseSteer = m_steerScaleTurbo; - // } else if (!turbo && slow) { - // speedToUseMove = m_moveScaleSlow; - // speedToUseSteer = m_steerScaleSlow; - // } else { - // speedToUseMove = m_moveScale; - // speedToUseSteer = m_steerScale; - // } - - // m_moveInput = - // OI.getInstance().getDriveTrainController().getLeftYAxis(); - // m_steerInput = - // OI.getInstance().getDriveTrainController().getRightXAxis(); - m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis(); - m_steerInput = OI.getInstance().getDriverController().getRightXAxis(); - - if (controlMode == DriveControlMode.JOYSTICK) { - m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - } - else if (controlMode == DriveControlMode.CLIMB) { - m_moveOutput = climbSpeed; - } - - if (useGyroLock) { // If currently in gyro lock mode, - if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) { // but stopped driving or started turning, - setGyroLock(false, false); // turn off gyro lock mode - } - } else { // If not yet in gyro lock mode, - if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) { // but just started driving without turning, - setGyroLock(true, false); // gyro lock to current heading - } - } - - if (useGyroLock) { - double yawError = gyroLockAngleDeg - getGyroAngleDeg(); - m_steerOutput = kPGyro * yawError; - } else { - m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - } - - m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75); - // break; - // case CONTROLLER_XBOX_ARCADE_RIGHT: - // m_moveInput = - // OI.getInstance().getDrivetrainController().getRightYAxis(); - // m_steerInput = - // OI.getInstance().getDrivetrainController().getRightXAxis(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // case CONTROLLER_XBOX_ARCADE_LEFT: - // m_moveInput = - // OI.getInstance().getDrivetrainController().getLeftYAxis(); - // m_steerInput = - // OI.getInstance().getDrivetrainController().getLeftXAxis(); - // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, - // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); - // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, - // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); - // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); - // break; - // } - } - - public void rawMoveSteer(double move, double steer) { - m_drive.arcadeDrive(move, steer, false); - } - - public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { - - if (elevatorRight.getSelectedSensorPosition(0) >= 3550) { - leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5); - rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5); - } - else /*(elevatorRight.getSelectedSensorPosition(0) < 3550)*/ { - leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput); - rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput); - } - } - - private boolean inDeadZone(double input) { - boolean inDeadZone; - if (Math.abs(input) < STICK_DEADBAND) { - inDeadZone = true; - } else { - inDeadZone = false; - } - return inDeadZone; - } - - public double adjustForSensitivity(double scale, double trim, - double steer, int nonLinearFactor, double wheelNonLinearity) { - if (inDeadZone(steer)) - return 0; - - steer += trim; - steer *= scale; - steer = limitValue(steer); - - int iterations = Math.abs(nonLinearFactor); - for (int i = 0; i < iterations; i++) { - if (nonLinearFactor > 0) { - steer = nonlinearStickCalcPositive(steer, wheelNonLinearity); - } else { - steer = nonlinearStickCalcNegative(steer, wheelNonLinearity); - } - } - return steer; - } - - private double limitValue(double value) { - if (value > 1.0) { - value = 1.0; - } else if (value < -1.0) { - value = -1.0; - } - return value; - } - - private double nonlinearStickCalcPositive(double steer, - double steerNonLinearity) { - return Math.sin(Math.PI / 2.0 * steerNonLinearity * steer) - / Math.sin(Math.PI / 2.0 * steerNonLinearity); - } - - private double nonlinearStickCalcNegative(double steer, - double steerNonLinearity) { - return Math.asin(steerNonLinearity * steer) - / Math.asin(steerNonLinearity); - } - - //public void setShiftState(SpeedShiftState state) { - // if(state == SpeedShiftState.HI) { - // speedShift.set(true); - // } - // else if(state == SpeedShiftState.LO) { - // speedShift.set(false); - // } - //} - - public synchronized boolean isFinished() { - return isFinished; - } - - public synchronized void setFinished(boolean isFinished) { - this.isFinished = isFinished; - } - - @Override - public void setPeriodMs(long periodMs) { - //mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers); - mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers); - mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers); - pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers); - mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers); - mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers); - adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers); - - this.periodMs = periodMs; - } - - public double getPeriodMs() { - return periodMs; - } - - public boolean isRed() { - return isRed; - } - - public void setIsRed(boolean status) { - isRed = status; - } - - public static double rotationsToInches(double rotations) { - return rotations * (Constants.kDriveWheelDiameterInches * Math.PI); - } - - public static double rpmToInchesPerSecond(double rpm) { - return rotationsToInches(rpm) / 60; - } - - public static double inchesToRotations(double inches) { - return inches / (Constants.kDriveWheelDiameterInches * Math.PI); - } - - public static double inchesPerSecondToRpm(double inches_per_second) { - return inchesToRotations(inches_per_second) * 60; - } - - public double getLeftPositionWorld() { - return leftDrive1.getPositionWorld(); - } - - public double getRightPositionWorld() { - return rightDrive1.getPositionWorld(); - } - - public void updateStatus(Robot.OperationMode operationMode) { - if (operationMode == Robot.OperationMode.TEST) { - try { - SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); - SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0)); - SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld()); - SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld()); - SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12); - SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12); - //SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID)); - //SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID)); -// SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID)); - //SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID)); - //SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID)); -// SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID)); - //SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn()); - //SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn()); - SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD); - //SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg()); - SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg()); - MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); - double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0; - SmartDashboard.putNumber("Gyro Delta", delta); - //SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating); - SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating()); - SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg); - SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg()); - SmartDashboard.putNumber("Steer Output", m_steerOutput); - SmartDashboard.putNumber("Move Output", m_moveOutput); - SmartDashboard.putNumber("Steer Input", m_steerInput); - SmartDashboard.putNumber("Move Input", m_moveInput); - } - catch (Exception e) { - System.err.println("Drivetrain update status error"); - } - } - else { - - } - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java deleted file mode 100644 index 129b561..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.usfirst.frc4388.robot.subsystems; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.robot.RobotMap; - -import edu.wpi.first.wpilibj.DoubleSolenoid; -import edu.wpi.first.wpilibj.command.Subsystem; - -public class Pnumatics extends Subsystem { - - - private DoubleSolenoid speedShift; - private DoubleSolenoid Intake; - - - public Pnumatics() { - try { - speedShift = new DoubleSolenoid(1,0,1); - Intake = new DoubleSolenoid(1,4,5 ); - } - catch (Exception e) { - System.err.println("An error occurred in the Pnumatics constructor"); - } - } - - public void setShiftState(boolean state) { - if (state==true) { - speedShift.set(DoubleSolenoid.Value.kForward); - } - if (state==false) { - speedShift.set(DoubleSolenoid.Value.kReverse); - } - } - public void setIntake(boolean state) { - if (state==true) { - Intake.set(DoubleSolenoid.Value.kForward); - } - if (state==false) { - Intake.set(DoubleSolenoid.Value.kReverse); - } - } - - public void initDefaultCommand() { - } -} - diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java deleted file mode 100644 index c217c81..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java +++ /dev/null @@ -1,228 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; -import java.util.Optional; -import java.util.Set; - -import org.usfirst.frc4388.robot.Constants; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.Timer; - -/** - * Implements an adaptive pure pursuit controller. See: - * https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4 - * .pdf - * - * Basically, we find a spot on the path we'd like to follow and calculate the - * wheel speeds necessary to make us land on that spot. The target spot is a - * specified distance ahead of us, and we look further ahead the greater our - * tracking error. - */ -public class AdaptivePurePursuitController { - private static final double kEpsilon = 1E-9; - - double mFixedLookahead; - Path mPath; - RigidTransform2d.Delta mLastCommand; - double mLastTime; - double mMaxAccel; - double mDt; - boolean mReversed; - double mPathCompletionTolerance; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - - public AdaptivePurePursuitController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - //motorController.configNominalOutputVoltage(+0.0f, -0.0f); - //motorController.configPeakOutputVoltage(+12.0f, -12.0f); - //motorController.setProfile(0); - motorController.config_kP(0, pidParams.kP, 0); - motorController.config_kI(0, pidParams.kI, 0); - motorController.config_kD(0, pidParams.kD, 0); - motorController.config_kF(0, pidParams.kF, 0); - motorController.configNominalOutputForward(+0.0f, 0); - motorController.configNominalOutputReverse(-0.0f, 0); - motorController.configPeakOutputForward(+1.0f, 0); - motorController.configPeakOutputReverse(-1.0f, 0); - motorController.selectProfileSlot(0, 0); - } - } - - public void setPath(double fixed_lookahead, double max_accel, Path path, - boolean reversed, double path_completion_tolerance) { - mFixedLookahead = fixed_lookahead; - mMaxAccel = max_accel; - mPath = path; - mDt = periodMs; - mLastCommand = null; - mReversed = reversed; - mPathCompletionTolerance = path_completion_tolerance; - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Speed); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Velocity, 0); - } - } - - public boolean isDone() { - double remainingLength = mPath.getRemainingLength(); - return remainingLength <= mPathCompletionTolerance; - } - - public boolean controlLoopUpdate(RigidTransform2d robot_pose) { - RigidTransform2d.Delta command = update(robot_pose, Timer.getFPGATimestamp()); - Kinematics.DriveVelocity setpoint = Kinematics.inverseKinematics(command); - - // Scale the command to respect the max velocity limits - double max_vel = 0.0; - max_vel = Math.max(max_vel, Math.abs(setpoint.left)); - max_vel = Math.max(max_vel, Math.abs(setpoint.right)); - if (max_vel > Constants.kPathFollowingMaxVel) { - double scaling = Constants.kPathFollowingMaxVel / max_vel; - setpoint = new Kinematics.DriveVelocity(setpoint.left * scaling, setpoint.right * scaling); - } - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - motorController.setVelocityWorld(-setpoint.right); - } - else { - motorController.setVelocityWorld(-setpoint.left); - } - } - - return isDone(); - } - - public RigidTransform2d.Delta update(RigidTransform2d robot_pose, double now) { - RigidTransform2d pose = robot_pose; - if (mReversed) { - pose = new RigidTransform2d(robot_pose.getTranslation(), - robot_pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI))); - } - - double distance_from_path = mPath.update(robot_pose.getTranslation()); - if (this.isDone()) { - return new RigidTransform2d.Delta(0, 0, 0); - } - - PathSegment.Sample lookahead_point = mPath.getLookaheadPoint(robot_pose.getTranslation(), - distance_from_path + mFixedLookahead); - Optional circle = joinPath(pose, lookahead_point.translation); - - double speed = lookahead_point.speed; - if (mReversed) { - speed *= -1; - } - // Ensure we don't accelerate too fast from the previous command - double dt = now - mLastTime; - if (mLastCommand == null) { - mLastCommand = new RigidTransform2d.Delta(0, 0, 0); - dt = mDt; - } - double accel = (speed - mLastCommand.dx) / dt; - if (accel < -mMaxAccel) { - speed = mLastCommand.dx - mMaxAccel * dt; - } else if (accel > mMaxAccel) { - speed = mLastCommand.dx + mMaxAccel * dt; - } - - // Ensure we slow down in time to stop - // vf^2 = v^2 + 2*a*d - // 0 = v^2 + 2*a*d - double remaining_distance = mPath.getRemainingLength(); - double max_allowed_speed = Math.sqrt(2 * mMaxAccel * remaining_distance); - if (Math.abs(speed) > max_allowed_speed) { - speed = max_allowed_speed * Math.signum(speed); - } - final double kMinSpeed = 4.0; - if (Math.abs(speed) < kMinSpeed) { - // Hack for dealing with problems tracking very low speeds with - // Talons - speed = kMinSpeed * Math.signum(speed); - } - - RigidTransform2d.Delta rv; - if (circle.isPresent()) { - rv = new RigidTransform2d.Delta(speed, 0, - (circle.get().turn_right ? -1 : 1) * Math.abs(speed) / circle.get().radius); - } else { - rv = new RigidTransform2d.Delta(speed, 0, 0); - } - mLastTime = now; - mLastCommand = rv; - return rv; - } - - public Set getMarkersCrossed() { - return mPath.getMarkersCrossed(); - } - - public static class Circle { - public final Translation2d center; - public final double radius; - public final boolean turn_right; - - public Circle(Translation2d center, double radius, boolean turn_right) { - this.center = center; - this.radius = radius; - this.turn_right = turn_right; - } - } - - public static Optional joinPath(RigidTransform2d robot_pose, Translation2d lookahead_point) { - double x1 = robot_pose.getTranslation().getX(); - double y1 = robot_pose.getTranslation().getY(); - double x2 = lookahead_point.getX(); - double y2 = lookahead_point.getY(); - - Translation2d pose_to_lookahead = robot_pose.getTranslation().inverse().translateBy(lookahead_point); - double cross_product = pose_to_lookahead.getX() * robot_pose.getRotation().sin() - - pose_to_lookahead.getY() * robot_pose.getRotation().cos(); - if (Math.abs(cross_product) < kEpsilon) { - return Optional.empty(); - } - - double dx = x1 - x2; - double dy = y1 - y2; - double my = (cross_product > 0 ? -1 : 1) * robot_pose.getRotation().cos(); - double mx = (cross_product > 0 ? 1 : -1) * robot_pose.getRotation().sin(); - - double cross_term = mx * dx + my * dy; - - if (Math.abs(cross_term) < kEpsilon) { - // Points are colinear - return Optional.empty(); - } - - return Optional.of(new Circle( - new Translation2d((mx * (x1 * x1 - x2 * x2 - dy * dy) + 2 * my * x1 * dy) / (2 * cross_term), - (-my * (-y1 * y1 + y2 * y2 + dx * dx) + 2 * mx * y1 * dx) / (2 * cross_term)), - .5 * Math.abs((dx * dx + dy * dy) / cross_term), cross_product > 0)); - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java deleted file mode 100644 index 8d3fc74..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.usfirst.frc4388.utility; - -public class BHRMathUtils { - - public static double normalizeAngle0To360(double currentAccumAngle) { - // reduce the angle - double normalizedAngle = currentAccumAngle % 360; - - // force it to be the positive remainder, so that 0 <= angle < 360 - normalizedAngle = (normalizedAngle + 360) % 360; - - return normalizedAngle; - } - - public static double adjustAccumAngleToDesired(double currentAccumAngle, double desiredAngle0To360) { - double normalizedAngle = normalizeAngle0To360(currentAccumAngle); - - if ( Math.abs(normalizedAngle - desiredAngle0To360) <= 180) { - double deltaAngle = normalizedAngle - desiredAngle0To360; - return currentAccumAngle - deltaAngle; - } - else { - double deltaAngle = desiredAngle0To360 - normalizedAngle; - if (deltaAngle < 0) { - deltaAngle += 360; - } - else { - deltaAngle -= 360; - } - return currentAccumAngle + deltaAngle; - } - } - - public static double adjustAccumAngleToClosest180(double currentAccumAngle) { - double normalizedAngle = Math.abs(normalizeAngle0To360(currentAccumAngle)); - - if (normalizedAngle < 90 || normalizedAngle > 270) { - return adjustAccumAngleToDesired(currentAccumAngle, 0); - } - else { - return adjustAccumAngleToDesired(currentAccumAngle, 180); - } - } - - public static void main(String[] args) { - System.out.println("Accum angle to desired 721 to 45 = " + adjustAccumAngleToDesired(721, 45)); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java deleted file mode 100644 index 52088f7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java +++ /dev/null @@ -1,65 +0,0 @@ -package org.usfirst.frc4388.utility; - -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -public class CANTalonEncoder extends WPI_TalonSRX -{ - private double encoderTicksToWorld; - private boolean isRight = true; - - - - public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) { - this(deviceNumber, encoderTicksToWorld, false, feedbackDevice); - } - - public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) { - super(deviceNumber); - //this.setFeedbackDevice(feedbackDevice); - this.configSelectedFeedbackSensor(feedbackDevice, 0, 0); - this.encoderTicksToWorld = encoderTicksToWorld; - this.isRight = isRight; - } - - public boolean isRight() { - return isRight; - } - - public void setRight(boolean isRight) { - this.isRight = isRight; - } - - public double convertEncoderTicksToWorld(double encoderTicks) { - return encoderTicks / encoderTicksToWorld; - } - - public double convertEncoderWorldToTicks(double worldValue) { - return worldValue * encoderTicksToWorld; - } - - public void setWorld(double worldValue) { - //this.set(convertEncoderWorldToTicks(worldValue)); - this.set(getControlMode(), convertEncoderWorldToTicks(worldValue)); //TODO: change to explicit mode set? - } - - public void setPositionWorld(double worldValue) { - //this.setPosition(convertEncoderWorldToTicks(worldValue)); - this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0); //TODO: verify - } - - public double getPositionWorld() { - //return convertEncoderTicksToWorld(this.getPosition()); - return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0)); //TODO: verify want 0="Primary closed-loop" - } - - public void setVelocityWorld(double worldValue) { - //this.set(convertEncoderWorldToTicks(worldValue) * 0.1); - this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1); //TODO: change to explicit mode set? - } - - public double getVelocityWorld() { - //return convertEncoderTicksToWorld(this.getSpeed() / 0.1); - return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1); //TODO: verify want 0="Primary closed-loop" - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java deleted file mode 100644 index 135bb97..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java +++ /dev/null @@ -1,199 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.io.File; -import java.io.FileReader; -import java.io.FileWriter; -import java.io.IOException; -import java.lang.reflect.Field; -import java.math.BigDecimal; -import java.util.ArrayList; -import java.util.Collection; -import java.util.HashMap; -import java.util.List; -import java.util.Set; - -import org.json.simple.JSONObject; -import org.json.simple.parser.JSONParser; -import org.json.simple.parser.ParseException; - -/** - * ConstantsBase - * - * Base class for storing robot constants. Anything stored as a public static - * field will be reflected and be able to set externally - */ -public abstract class ConstantsBase { - HashMap modifiedKeys = new HashMap(); - - public abstract String getFileLocation(); - - public static class Constant { - public String name; - public Class type; - public Object value; - - public Constant(String name, Class type, Object value) { - this.name = name; - this.type = type; - this.value = value; - } - - @Override - public boolean equals(Object o) { - String itsName = ((Constant) o).name; - Class itsType = ((Constant) o).type; - Object itsValue = ((Constant) o).value; - return o instanceof Constant && this.name.equals(itsName) && this.type.equals(itsType) - && this.value.equals(itsValue); - } - } - - public File getFile() { - String filePath = getFileLocation(); - filePath = filePath.replaceFirst("^~", System.getProperty("user.home")); - return new File(filePath); - } - - public boolean setConstant(String name, Double value) { - return setConstantRaw(name, value); - } - - public boolean setConstant(String name, Integer value) { - return setConstantRaw(name, value); - } - - public boolean setConstant(String name, String value) { - return setConstantRaw(name, value); - } - - private boolean setConstantRaw(String name, Object value) { - boolean success = false; - Field[] declaredFields = this.getClass().getDeclaredFields(); - for (Field field : declaredFields) { - if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { - try { - Object current = field.get(this); - field.set(this, value); - success = true; - if (!value.equals(current)) { - modifiedKeys.put(name, true); - } - } catch (IllegalArgumentException | IllegalAccessException e) { - System.out.println("Could not set field: " + name); - } - } - } - return success; - } - - public Object getValueForConstant(String name) throws Exception { - Field[] declaredFields = this.getClass().getDeclaredFields(); - for (Field field : declaredFields) { - if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { - try { - return field.get(this); - } catch (IllegalArgumentException | IllegalAccessException e) { - throw new Exception("Constant not found"); - } - } - } - throw new Exception("Constant not found"); - } - - public Constant getConstant(String name) { - Field[] declaredFields = this.getClass().getDeclaredFields(); - for (Field field : declaredFields) { - if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { - try { - return new Constant(field.getName(), field.getType(), field.get(this)); - } catch (IllegalArgumentException | IllegalAccessException e) { - e.printStackTrace(); - } - } - } - return new Constant("", Object.class, 0); - } - - public Collection getConstants() { - List constants = (List) getAllConstants(); - int stop = constants.size() - 1; - for (int i = 0; i < constants.size(); ++i) { - Constant c = constants.get(i); - if ("kEndEditableArea".equals(c.name)) { - stop = i; - } - } - return constants.subList(0, stop); - } - - private Collection getAllConstants() { - Field[] declaredFields = this.getClass().getDeclaredFields(); - List constants = new ArrayList(declaredFields.length); - for (Field field : declaredFields) { - if (java.lang.reflect.Modifier.isStatic(field.getModifiers())) { - Constant c; - try { - c = new Constant(field.getName(), field.getType(), field.get(this)); - constants.add(c); - } catch (IllegalArgumentException | IllegalAccessException e) { - e.printStackTrace(); - } - } - } - return constants; - } - - public JSONObject getJSONObjectFromFile() throws IOException, ParseException { - File file = getFile(); - if (file == null || !file.exists()) { - return new JSONObject(); - } - FileReader reader; - reader = new FileReader(file); - JSONParser jsonParser = new JSONParser(); - return (JSONObject) jsonParser.parse(reader); - } - - public void loadFromFile() { - try { - JSONObject jsonObject = getJSONObjectFromFile(); - Set keys = jsonObject.keySet(); - for (Object o : keys) { - String key = (String) o; - Object value = jsonObject.get(o); - if (value instanceof Long && getConstant(key).type.equals(int.class)) { - value = new BigDecimal((Long) value).intValueExact(); - } - setConstantRaw(key, value); - } - } catch (IOException e) { - e.printStackTrace(); - } catch (ParseException e) { - e.printStackTrace(); - } - } - - public void saveToFile() { - File file = getFile(); - if (file == null) { - return; - } - try { - JSONObject json = getJSONObjectFromFile(); - FileWriter writer = new FileWriter(file); - for (String key : modifiedKeys.keySet()) { - try { - Object value = getValueForConstant(key); - json.put(key, value); - } catch (Exception e) { - e.printStackTrace(); - } - } - writer.write(json.toJSONString()); - writer.close(); - } catch (IOException | ParseException e) { - e.printStackTrace(); - } - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java deleted file mode 100644 index c53b589..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java +++ /dev/null @@ -1,7 +0,0 @@ -package org.usfirst.frc4388.utility; - -public interface ControlLoopable -{ - public void controlLoopUpdate(); - public void setPeriodMs(long periodMs); -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java deleted file mode 100644 index ac7be21..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java +++ /dev/null @@ -1,79 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.Timer; -import java.util.TimerTask; -import java.util.Vector; - -/** - * ControlLooper.java - *

- * Runs several ControlLoopables simultaneously with one timing loop. - * Useful for running a bunch of control loops - * with only one Thread worth of overhead. - * - * Shamelessly stolen from team 254 2015 code and then slightly modified - * - * @author Tom Bottiglieri - */ - -public class ControlLooper -{ - private Timer controlLoopTimer; - private Vector loopables = new Vector(); - private long periodMs; - private String name; - - public ControlLooper(String name, long periodMs) { - this.name = name; - this.periodMs = periodMs; - } - - private class ControlLoopTask extends TimerTask { - private ControlLooper controlLooper; - - public ControlLoopTask(ControlLooper controlLooper) { - if (controlLooper == null) { - throw new NullPointerException("Given control looper was null"); - } - this.controlLooper = controlLooper; - } - - @Override - public void run() { - controlLooper.controlLoopUpdate(); - } - - } - - public String getName() { - return name; - } - - public void start() { - if (controlLoopTimer == null) { - controlLoopTimer = new Timer(); - controlLoopTimer.schedule(new ControlLoopTask(this), 0L, periodMs); - } - } - - public void stop() { - if (controlLoopTimer != null) { - controlLoopTimer.cancel(); - controlLoopTimer = null; - } - } - - private void controlLoopUpdate() { - for (int i = 0; i < loopables.size(); ++i) { - ControlLoopable c = loopables.elementAt(i); - if (c != null) { - c.controlLoopUpdate(); - } - } - } - - public void addLoopable(ControlLoopable c) { - loopables.addElement(c); - c.setPeriodMs(periodMs); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java deleted file mode 100644 index ea3af27..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java +++ /dev/null @@ -1,894 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.awt.AWTException; -import java.awt.BasicStroke; -import java.awt.Color; -import java.awt.Dimension; -import java.awt.FontMetrics; -import java.awt.Graphics; -import java.awt.Graphics2D; -import java.awt.Image; -import java.awt.Rectangle; -import java.awt.RenderingHints; -import java.awt.Robot; -import java.awt.Stroke; -import java.awt.Toolkit; -import java.awt.datatransfer.Clipboard; -import java.awt.datatransfer.ClipboardOwner; -import java.awt.datatransfer.DataFlavor; -import java.awt.datatransfer.Transferable; -import java.awt.datatransfer.UnsupportedFlavorException; -import java.awt.event.ActionEvent; -import java.awt.event.ActionListener; -import java.awt.event.MouseAdapter; -import java.awt.event.MouseEvent; -import java.awt.geom.AffineTransform; -import java.awt.geom.Ellipse2D; -import java.awt.geom.Line2D; -import java.awt.image.BufferedImage; -import java.io.IOException; -import java.text.DecimalFormat; -import java.util.LinkedList; - -import javax.swing.JFrame; -import javax.swing.JMenuItem; -import javax.swing.JPanel; -import javax.swing.JPopupMenu; - -/** - * This class is a basic plotting class using the Java AWT interface. It has basic features which allow the user - * to plot multiple graphs on one figure, control axis dimensions, and specify colors. - * - * This is by all means not an extensive plotter, but it will help visualize data very quickly and accurately. If - * a more robust plotting function is required, the user is encouraged to use Excel or Matlab. The purpose of this - * class is to be easy to use with enought automation to have nice graphs with minimal effort, but give the user - * control over as much as possible, so they can generate the perfect chart. - * - * The plotter also features the ability to capture screen shots directly from the right-click menu, this allows - * the user to copy and paste plots into reports or other documents rather quickly. - * - * This class holds an interface similar to that of Matlab. - * - * This class currently only supports scatterd line charts. - * - * @author Kevin Harrilal - * @email kevin@team2168.org - * @version 1 - * @date 9 Sept 2014 - * - */ - -class FalconLinePlot extends JPanel implements ClipboardOwner{ - - - private static final long serialVersionUID = 3205256608145459434L; - private final int yPAD = 60; //controls how far the X- and Y- axis lines are away from the window edge - private final int xPAD = 70; //controls how far the X- and Y- axis lines are away from the window edge - - private double upperXtic; - private double lowerXtic; - private double upperYtic; - private double lowerYtic; - private boolean yGrid; - private boolean xGrid; - - private double yMax; - private double yMin; - private double xMax; - private double xMin; - - private int yticCount; - private int xticCount; - private double xTicStepSize; - private double yTicStepSize; - - boolean userSetYTic; - boolean userSetXTic; - - private String xLabel; - private String yLabel; - private String titleLabel; - protected static int count = 0; - - JPopupMenu menu = new JPopupMenu("Popup"); - - //Link List to hold all different plots on one graph. - private LinkedList link; - - - /** - * Constructor which Plots only Y-axis data. - * @param yData is a array of doubles representing the Y-axis values of the data to be plotted. - */ - public FalconLinePlot(double[] yData) - { - this(null,yData,Color.red); - } - - public FalconLinePlot(double[] yData,Color lineColor, Color marker) - { - this(null,yData,lineColor,marker); - } - - /** - * Constructor which Plots chart based on provided x and y data. X and Y arrays must be of the same length. - * @param xData is an array of doubles representing the X-axis values of the data to be plotted. - * @param yData is an array of double representing the Y-axis values of the data to be plotted. - */ - public FalconLinePlot(double[] xData, double[] yData) - { - this(xData,yData,Color.red,null); - } - - /** - * Constructor which Plots chart based on provided x and y axis data. - * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data - * is the second dimension. - */ - public FalconLinePlot(double[][] data) - { - this(getXVector(data),getYVector(data),Color.red,null); - } - -/** - * Constructor which plots charts based on provided x and y axis data in a single two dimensional array. - * @param data is a 2D array of doubles of size Nx2 or 2xN. The plot assumes X is the first dimension, and y data - * is the second dimension. - * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint - * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to - * not have datapoint markers. - */ - public FalconLinePlot(double[][] data, Color lineColor, Color markerColor) - { - this(getXVector(data),getYVector(data),lineColor,markerColor); - } - - /** - * Constructor which plots charts based on provided x and y axis data provided as separate arrays. The user can also specify the color of the adjoining line. - * Data markers are not displayed. - * @param xData is an array of doubles representing the X-axis values of the data to be plotted. - * @param yData is an array of double representing the Y-axis values of the data to be plotted. - * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint - */ - public FalconLinePlot(double[] xData, double[] yData,Color lineColor) - { - this(xData,yData,lineColor,null); - } - - - - /** - * Constructor which plots charts based on provided x and y axis data, provided as separate arrays. The user - * can also specify the color of the adjoining line and the color of the datapoint maker. - * @param xData is an array of doubles representing the X-axis values of the data to be plotted. - * @param yData is an array of double representing the Y-axis values of the data to be plotted. - * @param lineColor is the color the user wishes to be displayed for the line connecting each datapoint - * @param markerColor is the color the user which to be used for the data point. Make this null if the user wishes to - * not have datapoint markers. - */ - public FalconLinePlot(double[] xData, double[] yData,Color lineColor, Color markerColor) - { - xLabel = "X axis"; - yLabel = "Y axis"; - titleLabel = "Title"; - - upperXtic = -Double.MAX_VALUE; - lowerXtic = Double.MAX_VALUE; - upperYtic = -Double.MAX_VALUE; - lowerYtic = Double.MAX_VALUE; - xticCount = -Integer.MAX_VALUE; - - this.userSetXTic = false; - this.userSetYTic = false; - - link = new LinkedList(); - - addData(xData, yData, lineColor,markerColor); - - count ++; - JFrame g = new JFrame("Figure " + count); - g.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); - g.add(this); - g.setSize(600,400); - g.setLocationByPlatform(true); - g.setVisible(true); - - menu(g,this); - } - - /** - * Adds a plot to an existing figure. - * @param yData is a array of doubles representing the Y-axis values of the data to be plotted. - * @param color is the color the user wishes to be displayed for the line connecting each datapoint - */ - - public void addData(double[] y, Color lineColor) - { - addData(y, lineColor, null); - } - - public void addData(double[] y, Color lineColor, Color marker) - { - //cant add y only data unless all other data is y only data - for(xyNode data: link) - if(data.x != null) - throw new Error ("All previous chart series need to have only Y data arrays"); - - addData(null,y,lineColor, marker); - } - - public void addData(double[] x, double[] y, Color lineColor) - { - addData(x,y,lineColor,null); - } - - - public void addData(double[][] data, Color lineColor) - { - addData(getXVector(data),getYVector(data),lineColor,null); - } - - public void addData(double[][] data, Color lineColor, Color marker) - { - addData(getXVector(data),getYVector(data),lineColor,marker); - } - - public void addData(double[] x, double[] y, Color lineColor, Color marker) - { - xyNode Data = new xyNode(); - - //copy y array into node - Data.y = new double[y.length]; - Data.lineColor = lineColor; - - if(marker == null) - Data.lineMarker = false; - else - { - Data.lineMarker = true; - Data.markerColor = marker; - } - for(int i=0; i list) - { - for(xyNode node: list) - { - double tempYMax = getMax(node.y); - double tempYMin = getMin(node.y); - - if(tempYMinyMax) - yMax=tempYMax; - - if(xticCount < node.y.length) - xticCount = node.y.length; - - - if(node.x != null) - { - double tempXMax = getMax(node.x); - double tempXMin = getMin(node.x); - - if(tempXMinxMax) - xMax=tempXMax; - - } - else - { - xMax=node.y.length-1; - xMin=0; - - } - - } - - } - - private double getMax(double[] data) { - double max = -Double.MAX_VALUE; - for(int i = 0; i < data.length; i++) { - if(data[i] > max) - max = data[i]; - } - return max; - } - - private double getMin(double[] data) { - double min = Double.MAX_VALUE; - for(int i = 0; i < data.length; i++) { - if(data[i] < min) - min = data[i]; - } - return min; - } - - public void setYLabel(String s) - { - yLabel = s; - } - - public void setXLabel(String s) - { - xLabel = s; - } - - public void setTitle(String s) - { - titleLabel = s; - } - - private void setYLabel(Graphics2D g2, String s) - { - FontMetrics fm = getFontMetrics(getFont()); - int width = fm.stringWidth(s); - - AffineTransform temp = g2.getTransform(); - - AffineTransform at = new AffineTransform(); - at.setToRotation(-Math.PI /2, 10, getHeight()/2+width/2); - g2.setTransform(at); - - //draw string in center of y axis - g2.drawString(s, 10, 7+getHeight()/2+width/2); - - g2.setTransform(temp); - - } - - private void setXLabel(Graphics2D g2, String s) - { - FontMetrics fm = getFontMetrics(getFont()); - int width = fm.stringWidth(s); - - g2.drawString(s, getWidth()/2-(width/2), getHeight()-10); - } - - private void setTitle(Graphics2D g2, String s) - { - FontMetrics fm = getFontMetrics(getFont()); - - String[] line = s.split("\n"); - - int height = xPAD/2 - ((line.length-1) * fm.getHeight()/2); - - for (int i=0; i - * The Type of Interpolable - * @see InterpolatingTreeMap - */ -public interface Interpolable { - /** - * Interpolates between this value and an other value according to a given - * parameter. If x is 0, the method should return this value. If x is 1, the - * method should return the other value. If 0 < x < 1, the return value - * should be interpolated proportionally between the two. - * - * @param other - * The value of the upper bound - * @param x - * The requested value. Should be between 0 and 1. - * @return Interpolable The estimated average between the surrounding - * data - */ - public T interpolate(T other, double x); -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java deleted file mode 100644 index 7c6de07..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java +++ /dev/null @@ -1,91 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.Map; -import java.util.TreeMap; - -/** - * Interpolating Tree Maps are used to get values at points that are not defined - * by making a guess from points that are defined. This uses linear - * interpolation. - * - * @param - * The type of the key (must implement InverseInterpolable) - * @param - * The type of the value (must implement Interpolable) - */ -public class InterpolatingTreeMap & Comparable, V extends Interpolable> - extends TreeMap { - private static final long serialVersionUID = 8347275262778054124L; - - int max_; - - public InterpolatingTreeMap(int maximumSize) { - max_ = maximumSize; - } - - public InterpolatingTreeMap() { - this(0); - } - - /** - * Inserts a key value pair, and trims the tree if a max size is specified - * - * @param key - * Key for inserted data - * @param value - * Value for inserted data - * @return the value - */ - @Override - public V put(K key, V value) { - if (max_ > 0 && max_ <= size()) { - // "Prune" the tree if it is oversize - K first = firstKey(); - remove(first); - } - - super.put(key, value); - - return value; - } - - @Override - public void putAll(Map map) { - System.out.println("Unimplemented Method"); - } - - /** - * - * @param key - * Lookup for a value (does not have to exist) - * @return V or null; V if it is Interpolable or exists, null if it is at a - * bound and cannot average - */ - public V getInterpolated(K key) { - V gotval = get(key); - if (gotval == null) { - /** Get surrounding keys for interpolation */ - K topBound = ceilingKey(key); - K bottomBound = floorKey(key); - - /** - * If attempting interpolation at ends of tree, return the nearest - * data point - */ - if (topBound == null && bottomBound == null) { - return null; - } else if (topBound == null) { - return get(bottomBound); - } else if (bottomBound == null) { - return get(topBound); - } - - /** Get surrounding values for interpolation */ - V topElem = get(topBound); - V bottomElem = get(bottomBound); - return bottomElem.interpolate(topElem, bottomBound.inverseInterpolate(topBound, key)); - } else { - return gotval; - } - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java deleted file mode 100644 index dc9f6a4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java +++ /dev/null @@ -1,25 +0,0 @@ -package org.usfirst.frc4388.utility; - -/** - * InverseInterpolable is an interface used by an Interpolating Tree as the Key - * type. Given two endpoint keys and a third query key, an InverseInterpolable - * object can calculate the interpolation parameter of the query key on the - * interval [0, 1]. - * - * @param - * The Type of InverseInterpolable - * @see InterpolatingTreeMap - */ -public interface InverseInterpolable { - /** - * Given this point (lower), a query point (query), and an upper point - * (upper), estimate how far (on [0, 1]) between 'lower' and 'upper' the - * query point lies. - * - * @param upper - * @param query - * @return The interpolation parameter on [0, 1] representing how far - * between this point and the upper point the query point lies. - */ - public double inverseInterpolate(T upper, T query); -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java deleted file mode 100644 index e3c3910..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java +++ /dev/null @@ -1,59 +0,0 @@ -package org.usfirst.frc4388.utility; - -import org.usfirst.frc4388.robot.Constants; - -/** - * Provides forward and inverse kinematics equations for the robot modeling the - * wheelbase as a differential drive (with a corrective factor to account for - * the inherent skidding of the center 4 wheels quasi-kinematically). - */ - -public class Kinematics { - private static final double kEpsilon = 1E-9; - - /** - * Forward kinematics using only encoders, rotation is implicit (less - * accurate than below, but useful for predicting motion) - */ - public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta) { - double linear_velocity = (left_wheel_delta + right_wheel_delta) / 2; - double delta_v = (right_wheel_delta - left_wheel_delta) / 2; - double delta_rotation = delta_v * 2 * Constants.kTrackScrubFactor / Constants.kTrackEffectiveDiameter; - return new RigidTransform2d.Delta(linear_velocity, 0, delta_rotation); - } - - /** - * Forward kinematics using encoders and explicitly measured rotation (ex. - * from gyro) - */ - public static RigidTransform2d.Delta forwardKinematics(double left_wheel_delta, double right_wheel_delta, - double delta_rotation_rads) { - return new RigidTransform2d.Delta((left_wheel_delta + right_wheel_delta) / 2, 0, delta_rotation_rads); - } - - /** Append the result of forward kinematics to a previous pose. */ - public static RigidTransform2d integrateForwardKinematics(RigidTransform2d current_pose, double left_wheel_delta, - double right_wheel_delta, Rotation2d current_heading) { - RigidTransform2d.Delta with_gyro = forwardKinematics(left_wheel_delta, right_wheel_delta, - current_pose.getRotation().inverse().rotateBy(current_heading).getRadians()); - return current_pose.transformBy(RigidTransform2d.fromVelocity(with_gyro)); - } - - public static class DriveVelocity { - public final double left; - public final double right; - - public DriveVelocity(double left, double right) { - this.left = left; - this.right = right; - } - } - - public static DriveVelocity inverseKinematics(RigidTransform2d.Delta velocity) { - if (Math.abs(velocity.dtheta) < kEpsilon) { - return new DriveVelocity(velocity.dx, velocity.dx); - } - double delta_v = Constants.kTrackEffectiveDiameter * velocity.dtheta / (2 * Constants.kTrackScrubFactor); - return new DriveVelocity(velocity.dx - delta_v, velocity.dx + delta_v); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java deleted file mode 100644 index 28dced9..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java +++ /dev/null @@ -1,137 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import org.usfirst.frc4388.robot.Constants; -import org.usfirst.frc4388.robot.subsystems.Drive; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -public class MMTalonPIDController -{ - protected static enum MMControlMode { STRAIGHT, TURN }; - public static enum MMTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY }; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected boolean useGyroLock; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - protected MMControlMode controlMode; - protected MMTalonTurnType turnType; - protected double targetValue; - - public MMTalonPIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - private int convertInchesPerSecToTicksPer100ms(double inchesPerSec) { - return (int)Math.round(Constants.kDriveEncoderTicksPerInch * inchesPerSec / 10); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kF(0, pidParams.kF, 0); //TODO: verify want parameter slot 0, with no timeout - } - } - - public void setMMStraightTarget(double startValue, double targetValue, double maxVelocityInchesPerSec, double maxAccelerationInchesPerSecPerSec, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { - controlMode = MMControlMode.STRAIGHT; - this.startGyroAngle = desiredAngle; - this.useGyroLock = useGyroLock; - this.targetValue = targetValue; - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - if (resetEncoder) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - //motorController.setMotionMagicCruiseVelocity(maxVelocity); - //motorController.setMotionMagicAcceleration(maxAcceleration); - //motorController.set(targetValue); - //motorController.changeControlMode(TalonControlMode.MotionMagic); - motorController.configMotionCruiseVelocity(convertInchesPerSecToTicksPer100ms(maxVelocityInchesPerSec), 0); - motorController.configMotionAcceleration(convertInchesPerSecToTicksPer100ms(maxAccelerationInchesPerSecPerSec), 0); - motorController.set(ControlMode.MotionMagic, targetValue); - } - } - - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - } - - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position - } - } - - private double calcTrackDistance(double deltaAngleDeg, MMTalonTurnType turnType, double trackWidth) { - double trackDistance = deltaAngleDeg / 360.0 * Math.PI * trackWidth; - if (turnType == MMTalonTurnType.TANK) { - return trackDistance; - } - else if (turnType == MMTalonTurnType.LEFT_SIDE_ONLY) { - return trackDistance * 2.0; - } - else if (turnType == MMTalonTurnType.RIGHT_SIDE_ONLY) { - return -trackDistance * 2.0; - } - return 0.0; - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - // Calculate the motion profile feed forward and gyro feedback terms - double rightTarget = 0.0; - double leftTarget = 0.0; - - // Update the set points - if (controlMode == MMControlMode.STRAIGHT) { - double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0; - double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, Drive.TRACK_WIDTH_INCHES); - rightTarget = targetValue + deltaDistance; - leftTarget = targetValue - deltaDistance; - - // Update the controllers with updated set points. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(rightTarget); - motorController.set(motorController.getControlMode(), rightTarget); //TODO: change to explicit mode set? - } - else { - //motorController.set(leftTarget); - motorController.set(motorController.getControlMode(), leftTarget); //TODO: change to explicit mode set? - } - } - } - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java deleted file mode 100644 index b91fc31..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java +++ /dev/null @@ -1,156 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -public class MPSoftwarePIDController -{ - public static enum MPSoftwareTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY, LEFT_ARC, RIGHT_ARC }; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected MotionProfileBoxCar mp; - protected MotionProfilePoint mpPoint; - protected boolean useGyroLock; - protected double startGyroAngle; - protected double targetGyroAngle; - protected MPSoftwareTurnType turnType; - - protected double prevError = 0.0; // the prior error (used to compute velocity) - protected double totalError = 0.0; // the sum of the errors for use in the integral calc - - public MPSoftwarePIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - } - - public void setMPTurnTarget(double startAngleDeg, double targetAngleDeg, double maxTurnRateDegPerSec, double t1, double t2, MPSoftwareTurnType turnType, double trackWidth) { - this.turnType = turnType; - this.startGyroAngle = startAngleDeg; - this.targetGyroAngle = targetAngleDeg; - this.useGyroLock = true; - - // Set up the motion profile - mp = new MotionProfileBoxCar(startAngleDeg, targetAngleDeg, maxTurnRateDegPerSec, periodMs, t1, t2); - // NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - - prevError = 0.0; - totalError = 0.0; - } - - //public void setMPTurnTarget(String key, MPSoftwareTurnType turnType, double trackWidth) { - // this.turnType = turnType; - // this.useGyroLock = true; - // - // // Set up the motion profile - // mp = MotionProfileCache.getInstance().getMP(key); - // this.startGyroAngle = mp.getStartDistance(); - // this.targetGyroAngle = mp.getTargetDistance(); - // - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - // - // prevError = 0.0; - // totalError = 0.0; - //} - - public boolean controlLoopUpdate() { - return controlLoopUpdate(0); - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - mpPoint = mp.getNextPoint(mpPoint); - - // Check if we are finished - if (mpPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and error feedback terms - double error = mpPoint.position - currentGyroAngle; - - if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) { - totalError += error; - } - else { - totalError = 0; - } - - double output = pidParams.kP * error + pidParams.kI * totalError + pidParams.kD * (error - prevError) + pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity; - prevError = error; - - // Update the controllers set point. - if (turnType == MPSoftwareTurnType.TANK) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.set(output); - motorController.set(ControlMode.PercentOutput, output); - } - } - else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); - } - else { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - } - } - else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - else { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); - } - } - } - else if (turnType == MPSoftwareTurnType.LEFT_ARC) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(1.0 * output); - motorController.set(ControlMode.PercentOutput, 1.0 * output); - } - else { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - } - } - else if (turnType == MPSoftwareTurnType.RIGHT_ARC) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - else { - //motorController.set(1.0 * output); - motorController.set(ControlMode.PercentOutput, 1.0 * output); - } - } - } - - return false; - } - - public MotionProfilePoint getCurrentPoint() { - return mpPoint; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java deleted file mode 100644 index 9a9d760..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java +++ /dev/null @@ -1,233 +0,0 @@ - -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -public class MPTalonPIDController -{ - protected static enum MPControlMode { STRAIGHT, TURN }; - public static enum MPTalonTurnType { TANK, LEFT_SIDE_ONLY, RIGHT_SIDE_ONLY }; - - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected MotionProfileBoxCar mp; - protected MotionProfilePoint mpPoint; - protected boolean useGyroLock; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - protected MPControlMode controlMode; - protected MPTalonTurnType turnType; - - public MPTalonPIDController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout - } - } - - public void setMPTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2) { - setMPStraightTarget(startValue, targetValue, maxVelocity, t1, t2, false, 0, false); - } - - public void setMPTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2, boolean resetEncoder) { - setMPStraightTarget(startValue, targetValue, maxVelocity, t1, t2, false, 0, resetEncoder); - } - - public void setMPStraightTarget(double startValue, double targetValue, double maxVelocity, double t1, double t2, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { - controlMode = MPControlMode.STRAIGHT; - this.startGyroAngle = desiredAngle; - this.useGyroLock = useGyroLock; - - // Set up the motion profile - mp = new MotionProfileBoxCar(startValue, targetValue, maxVelocity, periodMs, t1, t2); - for (CANTalonEncoder motorController : motorControllers) { - if (resetEncoder) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - //motorController.set(mp.getStartDistance()); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.set(ControlMode.Position, mp.getStartDistance()); - } - } - - //public void setMPStraightTarget(String key, boolean useGyroLock, double desiredAngle, boolean resetEncoder) { - // controlMode = MPControlMode.STRAIGHT; - // this.startGyroAngle = desiredAngle; - // this.useGyroLock = useGyroLock; - // - // // Set up the motion profile - // mp = MotionProfileCache.getInstance().getMP(key); - // for (CANTalonEncoder motorController : motorControllers) { - // if (resetEncoder) { - // motorController.setPosition(0); - // } - // motorController.set(mp.getStartDistance()); - // motorController.changeControlMode(TalonControlMode.Position); - // } - //} - - public void setMPTurnTarget(double startAngleDeg, double targetAngleDeg, double maxTurnRateDegPerSec, double t1, double t2, MPTalonTurnType turnType, double trackWidth) { - controlMode = MPControlMode.TURN; - this.turnType = turnType; - this.startGyroAngle = startAngleDeg; - this.targetGyroAngle = targetAngleDeg; - this.useGyroLock = true; - - trackDistance = calcTrackDistance(targetAngleDeg - startAngleDeg, turnType, trackWidth); - - // Set up the motion profile - mp = new MotionProfileBoxCar(0, trackDistance, maxTurnRateDegPerSec, periodMs, t1, t2); - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - - if (Math.abs(trackDistance) < 0.0001) { - trackDistance = 1; - } - } - - private double calcTrackDistance(double deltaAngleDeg, MPTalonTurnType turnType, double trackWidth) { - double trackDistance = deltaAngleDeg / 360.0 * Math.PI * trackWidth; - if (turnType == MPTalonTurnType.TANK) { - return trackDistance; - } - else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) { - return trackDistance * 2.0; - } - else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) { - return -trackDistance * 2.0; - } - return 0.0; - } - - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - } - - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position - } - } - - public boolean controlLoopUpdate() { - return controlLoopUpdate(0); - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - mpPoint = mp.getNextPoint(mpPoint); - - // Check if we are finished - if (mpPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and gyro feedback terms - double KfLeft = 0.0; - double KfRight = 0.0; - - // Update the set points and Kf gains - if (controlMode == MPControlMode.STRAIGHT) { - double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0; - if (Math.abs(mpPoint.position) > 0.001) { - //KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - //KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position; - - KfLeft = -(pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity - pidParams.kG * gyroDelta) / mpPoint.position; - } - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); - } - else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); - } - } - } - - else { - double mpAngle = startGyroAngle + ((targetGyroAngle - startGyroAngle) * mpPoint.position / trackDistance); - double gyroDelta = mpAngle - currentGyroAngle; - if (Math.abs(mpPoint.position) > 0.001) { - KfLeft = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - KfRight = (pidParams.kA * mpPoint.acceleration + pidParams.kV * mpPoint.velocity + pidParams.kG * gyroDelta) / mpPoint.position; - } - - for (CANTalonEncoder motorController : motorControllers) { - if (turnType == MPTalonTurnType.TANK) { - if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); - } - else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); - } - } - else if (turnType == MPTalonTurnType.LEFT_SIDE_ONLY) { - if (!motorController.isRight()) { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(mpPoint.position); - } - } - else if (turnType == MPTalonTurnType.RIGHT_SIDE_ONLY) { - if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(-mpPoint.position); - } - } - } - } - - return false; - } - - public MotionProfilePoint getCurrentPoint() { - return mpPoint; - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java deleted file mode 100644 index b9e8fca..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java +++ /dev/null @@ -1,113 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import jaci.pathfinder.Trajectory.Segment; - -public class MPTalonPIDPathController -{ - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected PathGenerator path; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - - public MPTalonPIDPathController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - motorController.config_kP(0, pidParams.kP, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kI(0, pidParams.kI, 0); //TODO: verify want parameter slot 0, with no timeout - motorController.config_kD(0, pidParams.kD, 0); //TODO: verify want parameter slot 0, with no timeout - } - } - - public void setMPPathTarget(PathGenerator path) { - this.path = path; - path.resetCounter(); - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - } - - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position - } - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - Segment leftPoint = path.getLeftPoint(); - Segment rightPoint = path.getRightPoint(); - - // Check if we are finished - if (leftPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and gyro feedback terms - double KfLeft = 0.0; - double KfRight = 0.0; - - // Update the set points and Kf gains - double gyroDelta = -path.getHeading() - currentGyroAngle; - if (Math.abs(leftPoint.position) > 0.001) { - KfLeft = (pidParams.kA * leftPoint.acceleration + pidParams.kV * leftPoint.velocity + pidParams.kG * gyroDelta) / leftPoint.position; - KfRight = (pidParams.kA * rightPoint.acceleration + pidParams.kV * rightPoint.velocity - pidParams.kG * gyroDelta) / rightPoint.position; - } - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.setF(KfRight); - motorController.config_kF(0, KfRight, 0); //TODO: verify - motorController.setWorld(rightPoint.position); - } - else { - //motorController.setF(KfLeft); - motorController.config_kF(0, KfLeft, 0); //TODO: verify - motorController.setWorld(leftPoint.position); - } - } - - path.incrementCounter(); - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java deleted file mode 100644 index 9863dc4..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java +++ /dev/null @@ -1,111 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import com.ctre.phoenix.motorcontrol.ControlMode; - -import jaci.pathfinder.Trajectory.Segment; - -public class MPTalonPIDPathVelocityController -{ - protected ArrayList motorControllers; - protected long periodMs; - protected PIDParams pidParams; - protected PathGenerator path; - protected double startGyroAngle; - protected double targetGyroAngle; - protected double trackDistance; - - public MPTalonPIDPathVelocityController(long periodMs, PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - this.periodMs = periodMs; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); - //motorController.setF(pidParams.kF); - //motorController.configNominalOutputVoltage(+0.0f, -0.0f); - //motorController.configPeakOutputVoltage(+12.0f, -12.0f); - //motorController.setProfile(0); - motorController.config_kP(0, pidParams.kP, 0); - motorController.config_kI(0, pidParams.kI, 0); - motorController.config_kD(0, pidParams.kD, 0); - motorController.config_kF(0, pidParams.kF, 0); - motorController.configNominalOutputForward(+0.0f, 0); - motorController.configNominalOutputReverse(-0.0f, 0); - motorController.configPeakOutputForward(+1.0f, 0); - motorController.configPeakOutputReverse(-1.0f, 0); - motorController.selectProfileSlot(0, 0); - } - } - - public void setMPPathTarget(PathGenerator path) { - this.path = path; - path.resetCounter(); - - // Set up the motion profile - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Speed); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Velocity, 0); - } - } - - public void setZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(0); - //motorController.set(0); - //motorController.changeControlMode(TalonControlMode.Position); - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - motorController.set(ControlMode.Position, 0); - } - } - - public void resetZeroPosition() { - for (CANTalonEncoder motorController : motorControllers) { - motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout - } - } - - public void resetZeroPosition(double angle) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.setPosition(angle); - motorController.setSelectedSensorPosition((int)angle, 0, 0); //TODO URGENT: convert angle to raw sensor position - } - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - Segment leftPoint = path.getLeftPoint(); - Segment rightPoint = path.getRightPoint(); - - // Check if we are finished - if (leftPoint == null) { - return true; - } - - // Calculate the motion profile feed forward and gyro feedback terms - double rightVelocity = rightPoint.velocity; - double leftVelocity = leftPoint.velocity; - - // Update the controllers Kf and set point. - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - motorController.setVelocityWorld(rightVelocity); - } - else { - motorController.setVelocityWorld(leftVelocity); - } - } - - path.incrementCounter(); - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java deleted file mode 100644 index eb2367c..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java +++ /dev/null @@ -1,184 +0,0 @@ -package org.usfirst.frc4388.utility; - -public class MotionProfileBoxCar -{ - public static double DEFAULT_T1 = 200; // millisecond - public static double DEFAULT_T2 = 100; // millisecond - - private double startDistance; // any distance unit - private double targetDistance; // any distance unit - private double maxVelocity; // velocity unit consistent with targetDistance - - // Accel profile - // - // 0 T2 T1 - // | | | - // _____ - // / \ - // / \___ - // \ / - // \____/ - - private double itp; // time between points millisecond - private double t1 = DEFAULT_T1; // time when accel starts back to 0. millisecond (typically use t1 = 2 * t2) - private double t2 = DEFAULT_T2; // time when accel = max accel. millisecond - - private double t4; - private int numFilter1Boxes; - private int numFilter2Boxes; - private int numPoints; - - private int numITP; - private double filter1; - private double filter2; - private double previousPosition; - private double previousVelocity; - private double deltaFilter1; - private double[] filter2Window; - private int windowIndex; - private int pointIndex; - - public MotionProfileBoxCar(double startDistance, double targetDistance, double maxVelocity, double itp) { - this(startDistance, targetDistance, maxVelocity, itp, DEFAULT_T1, DEFAULT_T2); - } - - public MotionProfileBoxCar(double startDistance, double targetDistance, double maxVelocity, double itp, double t1, double t2) { - this.startDistance = startDistance; - this.targetDistance = targetDistance; - this.maxVelocity = maxVelocity; - this.itp = itp; - this.t1 = t1; - this.t2 = t2; - - initializeProfile(); - } - - private void initializeProfile() { - // t4 is the time in ms it takes to get to the end point when at max velocity - t4 = Math.abs((targetDistance - startDistance)/maxVelocity) * 1000; - - // We need to make t4 an even multiple of itp - t4 = (int)(itp * Math.ceil(t4/itp)); - - // In the case where t4 is less than the accel times, we need to adjust the - // accel times down so the filters works out. Lots of ways to do this but - // to keep things simple we will make t4 slightly larger than the sum of - // the accel times. - if (t4 < t1 + t2) { - double total = t1 + t2 + t4; - double t1t2Ratio = t1/t2; - double t2Adjusted = Math.floor(total / 2 / (1 + t1t2Ratio) / itp); - if (t2Adjusted % 2 != 0) { - t2Adjusted -= 1; - } - t2 = t2Adjusted * itp; - t1 = t2 * t1t2Ratio; - t4 = total - t1 - t2; - } - - // Adjust max velocity so that the end point works out to the correct position. - maxVelocity = Math.abs((targetDistance - startDistance) / t4) * 1000; - - numFilter1Boxes = (int)Math.ceil(t1/itp); - numFilter2Boxes = (int)Math.ceil(t2/itp); - numPoints = (int)Math.ceil(t4/itp); - - numITP = numPoints + numFilter1Boxes + numFilter2Boxes; - filter1 = 0; - filter2 = 0; - previousVelocity = 0; - previousPosition = startDistance; - deltaFilter1 = 1.0/numFilter1Boxes; - filter2Window = new double[numFilter2Boxes]; - windowIndex = 0; - pointIndex = 0; - if (startDistance > targetDistance && maxVelocity > 0) { - maxVelocity = -maxVelocity; - } - } - - public MotionProfilePoint getNextPoint(MotionProfilePoint point) { - if (point == null) { - point = new MotionProfilePoint(); - } - - if (pointIndex == 0) { - point.initialize(startDistance); - pointIndex++; - return point; - } - else if (pointIndex > numITP) { - return null; - } - - int input = (pointIndex - 1) < numPoints ? 1 : 0; - if (input > 0) { - filter1 = Math.min(1, filter1 + deltaFilter1); - } - else { - filter1 = Math.max(0, filter1 - deltaFilter1); - } - - double firstFilter1InWindow = filter2Window[windowIndex]; - if (pointIndex <= numFilter2Boxes) { - firstFilter1InWindow = 0; - } - filter2Window[windowIndex] = filter1; - - filter2 += (filter1 - firstFilter1InWindow) / numFilter2Boxes; - - point.time = pointIndex * itp / 1000.0; - point.velocity = filter2 * maxVelocity; - point.position = previousPosition + (point.velocity + previousVelocity) / 2 * itp / 1000; - point.acceleration = (point.velocity - previousVelocity) / itp * 1000; - - previousVelocity = point.velocity; - previousPosition = point.position; - windowIndex++; - if (windowIndex == numFilter2Boxes) { - windowIndex = 0; - } - - pointIndex++; - - return point; - } - - public double getStartDistance() { - return startDistance; - } - - public double getTargetDistance() { - return targetDistance; - } - - public double getMaxVelocity() { - return maxVelocity; - } - - public double getItp() { - return itp; - } - - public double getT1() { - return t1; - } - - public double getT2() { - return t2; - } - - public static void main(String[] args) { - long startTime = System.nanoTime(); - - MotionProfileBoxCar mp = new MotionProfileBoxCar(0, 96, 120, 10, 600, 300); - System.out.println("Time, Position, Velocity, Acceleration"); - MotionProfilePoint point = new MotionProfilePoint(); - while(mp.getNextPoint(point) != null) { - System.out.println(point.time + ", " + point.position + ", " + point.velocity + ", " + point.acceleration); - } - - long deltaTime = System.nanoTime() - startTime; - System.out.println("Time Box Car = " + (double)deltaTime * 1E-6 + " ms"); - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java deleted file mode 100644 index 358138b..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java +++ /dev/null @@ -1,15 +0,0 @@ -package org.usfirst.frc4388.utility; - -public class MotionProfilePoint { - public double time; - public double position; - public double velocity; - public double acceleration; - - public void initialize(double startPosition) { - time = 0; - position = startPosition; - velocity = 0; - acceleration = 0; - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java deleted file mode 100644 index c989f65..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.usfirst.frc4388.utility; - -public class PIDParams -{ - public double kP = 0; - public double kI = 0; - public double kD = 0; - public double kF = 0; - public double kA = 0; - public double kV = 0; - public double kG = 0; - public double iZone = 0; - public double rampRatePID = 0; - - public PIDParams() { - } - - public PIDParams(double kP) - { - this.kP = kP; - } - - public PIDParams(double kP, double kI, double kD) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - } - - public PIDParams(double kP, double kI, double kD, double kF) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kF = kF; - } - - public PIDParams(double kP, double kI, double kD, double kA, double kV) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kA = kA; - this.kV = kV; - } - - public PIDParams(double kP, double kI, double kD, double kA, double kV, double kG) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kA = kA; - this.kV = kV; - this.kG = kG; - } - - public PIDParams(double kP, double kI, double kD, double kA, double kV, double kG, double iZone) { - this.kP = kP; - this.kI = kI; - this.kD = kD; - this.kA = kA; - this.kV = kV; - this.kG = kG; - this.iZone = iZone; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java deleted file mode 100644 index 163c52d..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java +++ /dev/null @@ -1,260 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; -import java.util.HashSet; -import java.util.Iterator; -import java.util.List; -import java.util.Optional; -import java.util.Set; - -import org.usfirst.frc4388.utility.Path.Waypoint; - -/** - * A Path is a recording of the path that the robot takes. Path objects consist - * of a List of Waypoints that the robot passes by. Using multiple Waypoints in - * a Path object and the robot's current speed, the code can extrapolate future - * Waypoints and predict the robot's motion. It can also dictate the robot's - * motion along the set path. - */ -public class Path { - protected static final double kSegmentCompletePercentage = .99; - - protected List mWaypoints; - protected List mSegments; - protected Set mMarkersCrossed; - - /** - * A point along the Path, which consists of the location, the speed, and a - * string marker (that future code can identify). Paths consist of a List of - * Waypoints. - */ - public static class Waypoint { - public final Translation2d position; - public final double speed; - public final Optional marker; - - public Waypoint(Translation2d position, double speed) { - this.position = position; - this.speed = speed; - this.marker = Optional.empty(); - } - - public Waypoint(Translation2d position, double speed, String marker) { - this.position = position; - this.speed = speed; - this.marker = Optional.of(marker); - } - } - - public Path(List waypoints) { - mMarkersCrossed = new HashSet(); - mWaypoints = waypoints; - mSegments = new ArrayList(); - for (int i = 0; i < waypoints.size() - 1; ++i) { - mSegments.add( - new PathSegment(waypoints.get(i).position, waypoints.get(i + 1).position, waypoints.get(i).speed)); - } - // The first waypoint is already complete - if (mWaypoints.size() > 0) { - Waypoint first_waypoint = mWaypoints.get(0); - if (first_waypoint.marker.isPresent()) { - mMarkersCrossed.add(first_waypoint.marker.get()); - } - mWaypoints.remove(0); - } - } - - /** - * @param An - * initial position - * @return Returns the distance from the position to the first point on the - * path - */ - public double update(Translation2d position) { - double rv = 0.0; - for (Iterator it = mSegments.iterator(); it.hasNext();) { - PathSegment segment = it.next(); - PathSegment.ClosestPointReport closest_point_report = segment.getClosestPoint(position); - if (closest_point_report.index >= kSegmentCompletePercentage) { - it.remove(); - if (mWaypoints.size() > 0) { - Waypoint waypoint = mWaypoints.get(0); - if (waypoint.marker.isPresent()) { - mMarkersCrossed.add(waypoint.marker.get()); - } - mWaypoints.remove(0); - } - } else { - if (closest_point_report.index > 0.0) { - // Can shorten this segment - segment.updateStart(closest_point_report.closest_point); - } - // We are done - rv = closest_point_report.distance; - // ...unless the next segment is closer now - if (it.hasNext()) { - PathSegment next = it.next(); - PathSegment.ClosestPointReport next_closest_point_report = next.getClosestPoint(position); - if (next_closest_point_report.index > 0 - && next_closest_point_report.index < kSegmentCompletePercentage - && next_closest_point_report.distance < rv) { - next.updateStart(next_closest_point_report.closest_point); - rv = next_closest_point_report.distance; - mSegments.remove(0); - if (mWaypoints.size() > 0) { - Waypoint waypoint = mWaypoints.get(0); - if (waypoint.marker.isPresent()) { - mMarkersCrossed.add(waypoint.marker.get()); - } - mWaypoints.remove(0); - } - } - } - break; - } - } - return rv; - } - - public Set getMarkersCrossed() { - return mMarkersCrossed; - } - - public double getRemainingLength() { - double length = 0.0; - for (int i = 0; i < mSegments.size(); ++i) { - length += mSegments.get(i).getLength(); - } - return length; - } - - /** - * @param The - * robot's current position - * @param A - * specified distance to predict a future waypoint - * @return A segment of the robot's predicted motion with start/end points - * and speed. - */ - public PathSegment.Sample getLookaheadPoint(Translation2d position, double lookahead_distance) { - if (mSegments.size() == 0) { - return new PathSegment.Sample(new Translation2d(), 0); - } - - // Check the distances to the start and end of each segment. As soon as - // we find a point > lookahead_distance away, we know the right point - // lies somewhere on that segment. - Translation2d position_inverse = position.inverse(); - if (position_inverse.translateBy(mSegments.get(0).getStart()).norm() >= lookahead_distance) { - // Special case: Before the first point, so just return the first - // point. - return new PathSegment.Sample(mSegments.get(0).getStart(), mSegments.get(0).getSpeed()); - } - for (int i = 0; i < mSegments.size(); ++i) { - PathSegment segment = mSegments.get(i); - double distance = position_inverse.translateBy(segment.getEnd()).norm(); - if (distance >= lookahead_distance) { - // This segment contains the lookahead point - Optional intersection_point = getFirstCircleSegmentIntersection(segment, position, - lookahead_distance); - if (intersection_point.isPresent()) { - return new PathSegment.Sample(intersection_point.get(), segment.getSpeed()); - } else { - System.out.println("ERROR: No intersection point?"); - } - } - } - // Special case: After the last point, so extrapolate forward. - PathSegment last_segment = mSegments.get(mSegments.size() - 1); - PathSegment new_last_segment = new PathSegment(last_segment.getStart(), last_segment.interpolate(10000), - last_segment.getSpeed()); - Optional intersection_point = getFirstCircleSegmentIntersection(new_last_segment, position, - lookahead_distance); - if (intersection_point.isPresent()) { - return new PathSegment.Sample(intersection_point.get(), last_segment.getSpeed()); - } else { - System.out.println("ERROR: No intersection point anywhere on line?"); - return new PathSegment.Sample(last_segment.getEnd(), last_segment.getSpeed()); - } - } - - static Optional getFirstCircleSegmentIntersection(PathSegment segment, Translation2d center, - double radius) { - double x1 = segment.getStart().getX() - center.getX(); - double y1 = segment.getStart().getY() - center.getY(); - double x2 = segment.getEnd().getX() - center.getX(); - double y2 = segment.getEnd().getY() - center.getY(); - double dx = x2 - x1; - double dy = y2 - y1; - double dr_squared = dx * dx + dy * dy; - double det = x1 * y2 - x2 * y1; - - double discriminant = dr_squared * radius * radius - det * det; - if (discriminant < 0) { - // No intersection - return Optional.empty(); - } - - double sqrt_discriminant = Math.sqrt(discriminant); - Translation2d pos_solution = new Translation2d( - (det * dy + (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(), - (-det * dx + Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY()); - Translation2d neg_solution = new Translation2d( - (det * dy - (dy < 0 ? -1 : 1) * dx * sqrt_discriminant) / dr_squared + center.getX(), - (-det * dx - Math.abs(dy) * sqrt_discriminant) / dr_squared + center.getY()); - - // Choose the one between start and end that is closest to start - double pos_dot_product = segment.dotProduct(pos_solution); - double neg_dot_product = segment.dotProduct(neg_solution); - if (pos_dot_product < 0 && neg_dot_product >= 0) { - return Optional.of(neg_solution); - } else if (pos_dot_product >= 0 && neg_dot_product < 0) { - return Optional.of(pos_solution); - } else { - if (Math.abs(pos_dot_product) <= Math.abs(neg_dot_product)) { - return Optional.of(pos_solution); - } else { - return Optional.of(neg_solution); - } - } - } - - public static void addCircleArc(List waypoints, double radius, double angleDeg, int numPoints, String endMarker ) { - Waypoint last = waypoints.get(waypoints.size() - 1); - - double centerX = last.position.x_; - double centerY = radius; - - double deltaAngle = angleDeg / numPoints; - double currentAngle = deltaAngle; - for (int i = 0; i < numPoints; i++ ) { - double x = radius * Math.sin(Math.toRadians(currentAngle)) + centerX; - double y = centerY - radius * Math.cos(Math.toRadians(currentAngle)); - - if (i == numPoints - 1 && endMarker != null) { - Waypoint point = new Waypoint(new Translation2d(x, y), last.speed, endMarker); - waypoints.add(point); - } - else { - Waypoint point = new Waypoint(new Translation2d(x, y), last.speed); - waypoints.add(point); - } - - currentAngle += deltaAngle; - } - } - - public static void main(String[] args) { - - List waypoints = new ArrayList<>(); - waypoints.add(new Waypoint(new Translation2d(0, 0), 40.0)); - waypoints.add(new Waypoint(new Translation2d(-35, 0), 40.0)); - Path.addCircleArc(waypoints, 30.0, -45.0, 10, "hopperSensorOn"); - waypoints.add(new Waypoint(new Translation2d(-85, 30), 40.0)); - - for (int i = 0; i < waypoints.size(); i++) { - Waypoint curPoint = waypoints.get(i); - System.out.println("x = " + curPoint.position.x_ + ", y = " + curPoint.position.y_); - } - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java deleted file mode 100644 index f99df73..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java +++ /dev/null @@ -1,232 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.awt.Color; - -import org.usfirst.frc4388.robot.subsystems.Drive; - -import jaci.pathfinder.Pathfinder; -import jaci.pathfinder.Trajectory; -import jaci.pathfinder.Trajectory.Segment; -import jaci.pathfinder.Waypoint; -import jaci.pathfinder.modifiers.TankModifier; - -public class PathGenerator { - - private Segment[] centerPoints; - private Segment[] leftPoints; - private Segment[] rightPoints; - private int curIndex = 0; - - public PathGenerator(Waypoint[] points, double timeStep, double maxVelocity, double maxAccel, double maxJerk) { - - boolean negativeFlag = false; - for (int i = 0; i < points.length; i++) { - if (points[i].x < 0) { - negativeFlag = true; - } - } - if (negativeFlag == true) { - for (int i = 0; i < points.length; i++) { - points[i].x = -(points[i].x); - } - } - - Trajectory.Config config = new Trajectory.Config(Trajectory.FitMethod.HERMITE_CUBIC, Trajectory.Config.SAMPLES_LOW, timeStep, maxVelocity, maxAccel, maxJerk); - Trajectory trajectory = Pathfinder.generate(points, config); - centerPoints = trajectory.segments; - - TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES); - leftPoints = modifier.getLeftTrajectory().segments; - rightPoints = modifier.getRightTrajectory().segments; - - if (negativeFlag == true) { - for (int i = 0; i < centerPoints.length; i++) { - Segment curSeg = centerPoints[i]; - curSeg.x = -(curSeg.x); - curSeg.y = -(curSeg.y); - curSeg.jerk = -(curSeg.jerk); - curSeg.acceleration = -(curSeg.acceleration); - curSeg.velocity = -(curSeg.velocity); - curSeg.position = -(curSeg.position); - curSeg.heading = -(curSeg.heading); - curSeg = leftPoints[i]; - curSeg.x = -(curSeg.x); - curSeg.y = -(curSeg.y); - curSeg.jerk = -(curSeg.jerk); - curSeg.acceleration = -(curSeg.acceleration); - curSeg.velocity = -(curSeg.velocity); - curSeg.position = -(curSeg.position); - curSeg.heading = -(curSeg.heading); - curSeg = rightPoints[i]; - curSeg.x = -(curSeg.x); - curSeg.y = -(curSeg.y); - curSeg.jerk = -(curSeg.jerk); - curSeg.acceleration = -(curSeg.acceleration); - curSeg.velocity = -(curSeg.velocity); - curSeg.position = -(curSeg.position); - curSeg.heading = -(curSeg.heading); - } - } - -// TankModifier2 modifier2 = new TankModifier2(); -// modifier2.modify(centerPoints, Drive.TRACK_WIDTH_INCHES); -// leftPoints = modifier2.getLeftSegments(); -// rightPoints = modifier2.getRightSegments(); - } - - public Segment getLeftPoint() { - return (curIndex < leftPoints.length) ? leftPoints[curIndex] : null; - } - - public Segment getRightPoint() { - return (curIndex < rightPoints.length) ? rightPoints[curIndex] : null; - } - - public Segment getCenterPoint() { - return (curIndex < centerPoints.length) ? centerPoints[curIndex] : null; - } - - public Double getHeading() { - if (curIndex < centerPoints.length) { - double heading = Math.toDegrees(centerPoints[curIndex].heading); - if (heading > 180) { - heading -= 360; - } - else if (heading < -180) { - heading += 360; - } - return heading; - } - return null; - } - - public void incrementCounter() { - curIndex++; - } - - public void resetCounter() { - curIndex =0; - } - - public Segment[] getCenterPoints() { - return centerPoints; - } - - public Segment[] getLeftPoints() { - return leftPoints; - } - - public Segment[] getRightPoints() { - return rightPoints; - } - - public static void main(String[] args) { - Waypoint[] points = new Waypoint[] { - new Waypoint(0, 0, 0), - new Waypoint(78, 20, Pathfinder.d2r(32)) - }; - - PathGenerator path = new PathGenerator(points, 0.01, 120, 200.0, 700.0); - Segment[] centerSegments = path.getCenterPoints(); - Segment[] leftSegments = path.getLeftPoints(); - Segment[] rightSegments = path.getRightPoints(); - - double[] heading = new double[centerSegments.length]; - double[] centerX = new double[centerSegments.length]; - double[] centerY = new double[centerSegments.length]; - double[] centerAccel = new double[centerSegments.length]; - double[] centerVelocity = new double[centerSegments.length]; - double[] centerPosition = new double[centerSegments.length]; - double[] leftX = new double[leftSegments.length]; - double[] leftY = new double[leftSegments.length]; - double[] leftAccel = new double[leftSegments.length]; - double[] leftVelocity = new double[leftSegments.length]; - double[] leftPosition = new double[leftSegments.length]; - double[] rightX = new double[rightSegments.length]; - double[] rightY = new double[rightSegments.length]; - double[] rightAccel = new double[rightSegments.length]; - double[] rightVelocity = new double[rightSegments.length]; - double[] rightPosition = new double[rightSegments.length]; - - path.resetCounter(); - for (int i = 0; i < centerSegments.length; i++) { - heading[i] = path.getHeading(); - centerX[i] = path.getCenterPoint().x; - centerY[i] = path.getCenterPoint().y; - centerAccel[i] = path.getCenterPoint().acceleration; - centerVelocity[i] = path.getCenterPoint().velocity; - centerPosition[i] = path.getCenterPoint().position; - leftX[i] = path.getLeftPoint().x; - leftY[i] = path.getLeftPoint().y; - leftAccel[i] = path.getLeftPoint().acceleration; - leftVelocity[i] = path.getLeftPoint().velocity; - leftPosition[i] = path.getLeftPoint().position; - rightX[i] = path.getRightPoint().x; - rightY[i] = path.getRightPoint().y; - rightAccel[i] = path.getRightPoint().acceleration; - rightVelocity[i] = path.getRightPoint().velocity; - rightPosition[i] = path.getRightPoint().position; - path.incrementCounter(); - } - - FalconLinePlot fig4 = new FalconLinePlot(centerX); - fig4.yGridOn(); - fig4.xGridOn(); - fig4.setYLabel("X (in)"); - fig4.setXLabel("time"); - fig4.setTitle("Position Profile X"); - fig4.addData(rightX, Color.magenta); - fig4.addData(leftX, Color.blue); - - FalconLinePlot fig0 = new FalconLinePlot(centerY); - fig0.yGridOn(); - fig0.xGridOn(); - fig0.setYLabel("Y (in)"); - fig0.setXLabel("time"); - fig0.setTitle("Position Profile Y"); - fig0.addData(rightY, Color.magenta); - fig0.addData(leftY, Color.blue); - - FalconLinePlot fig1 = new FalconLinePlot(centerPosition); - fig1.yGridOn(); - fig1.xGridOn(); - fig1.setYLabel("Position (in)"); - fig1.setXLabel("time (seconds)"); - fig1.setTitle("Position Profile for Left and Right Wheels \n Left = Blue, Right = Magenta"); - fig1.addData(rightPosition, Color.magenta); - fig1.addData(leftPosition, Color.blue); - - FalconLinePlot fig2 = new FalconLinePlot(centerVelocity, Color.green, Color.green); - fig2.yGridOn(); - fig2.xGridOn(); - fig2.setYLabel("Velocity (in/sec)"); - fig2.setXLabel("time (seconds)"); - fig2.setTitle("Velocity Profile for Left and Right Wheels \n Left = Blue, Right = Magenta"); - fig2.addData(rightVelocity, Color.magenta); - fig2.addData(leftVelocity, Color.blue); - - FalconLinePlot fig3 = new FalconLinePlot(centerAccel, Color.green, Color.green); - fig3.yGridOn(); - fig3.xGridOn(); - fig3.setYLabel("Accel (in/sec^2)"); - fig3.setXLabel("time (seconds)"); - fig3.setTitle("Accel Profile for Left and Right Wheels \n Left = Blue, Right = Magenta"); - fig3.addData(rightAccel, Color.magenta); - fig3.addData(leftAccel, Color.blue); - - FalconLinePlot fig5 = new FalconLinePlot(heading); - fig5.yGridOn(); - fig5.xGridOn(); - fig5.setYLabel("Heading"); - fig5.setXLabel("time"); - fig5.setTitle("Heading Profile"); - - FalconLinePlot fig6 = new FalconLinePlot(centerX, centerY); - fig6.yGridOn(); - fig6.xGridOn(); - fig6.setYLabel("Y"); - fig6.setXLabel("X"); - fig6.setTitle("XY Profile"); - } - -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java deleted file mode 100644 index df9499d..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java +++ /dev/null @@ -1,89 +0,0 @@ -package org.usfirst.frc4388.utility; - -/** - * A PathSegment consists of two Translation2d objects (the start and end - * points) as well as the speed of the robot. - * - */ -public class PathSegment { - protected static final double kEpsilon = 1E-9; - - public static class Sample { - public final Translation2d translation; - public final double speed; - - public Sample(Translation2d translation, double speed) { - this.translation = translation; - this.speed = speed; - } - } - - protected double mSpeed; - protected Translation2d mStart; - protected Translation2d mEnd; - protected Translation2d mStartToEnd; // pre-computed for efficiency - protected double mLength; // pre-computed for efficiency - - public static class ClosestPointReport { - public double index; // Index of the point on the path segment (not - // clamped to [0, 1]) - public double clamped_index; // As above, but clamped to [0, 1] - public Translation2d closest_point; // The result of - // interpolate(clamped_index) - public double distance; // The distance from closest_point to the query - // point - } - - public PathSegment(Translation2d start, Translation2d end, double speed) { - mEnd = end; - mSpeed = speed; - updateStart(start); - } - - public void updateStart(Translation2d new_start) { - mStart = new_start; - mStartToEnd = mStart.inverse().translateBy(mEnd); - mLength = mStartToEnd.norm(); - } - - public double getSpeed() { - return mSpeed; - } - - public Translation2d getStart() { - return mStart; - } - - public Translation2d getEnd() { - return mEnd; - } - - public double getLength() { - return mLength; - } - - // Index is on [0, 1] - public Translation2d interpolate(double index) { - return mStart.interpolate(mEnd, index); - } - - public double dotProduct(Translation2d other) { - Translation2d start_to_other = mStart.inverse().translateBy(other); - return mStartToEnd.getX() * start_to_other.getX() + mStartToEnd.getY() * start_to_other.getY(); - } - - public ClosestPointReport getClosestPoint(Translation2d query_point) { - ClosestPointReport rv = new ClosestPointReport(); - if (mLength > kEpsilon) { - double dot_product = dotProduct(query_point); - rv.index = dot_product / (mLength * mLength); - rv.clamped_index = Math.min(1.0, Math.max(0.0, rv.index)); - rv.closest_point = interpolate(rv.index); - } else { - rv.index = rv.clamped_index = 0.0; - rv.closest_point = new Translation2d(mStart); - } - rv.distance = rv.closest_point.inverse().translateBy(query_point).norm(); - return rv; - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java deleted file mode 100644 index c48b552..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java +++ /dev/null @@ -1,131 +0,0 @@ -package org.usfirst.frc4388.utility; - -/** - * Represents a 2d pose (rigid transform) containing translational and - * rotational elements. - * - * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) - */ -public class RigidTransform2d implements Interpolable { - private final static double kEps = 1E-9; - - // Movement along an arc at constant curvature and velocity. We can use - // ideas from "differential calculus" to create new RigidTransform2d's from - // a Delta. - public static class Delta { - public final double dx; - public final double dy; - public final double dtheta; - - public Delta(double dx, double dy, double dtheta) { - this.dx = dx; - this.dy = dy; - this.dtheta = dtheta; - } - } - - protected Translation2d translation_; - protected Rotation2d rotation_; - - public RigidTransform2d() { - translation_ = new Translation2d(); - rotation_ = new Rotation2d(); - } - - public RigidTransform2d(Translation2d translation, Rotation2d rotation) { - translation_ = translation; - rotation_ = rotation; - } - - public RigidTransform2d(RigidTransform2d other) { - translation_ = new Translation2d(other.translation_); - rotation_ = new Rotation2d(other.rotation_); - } - - public static RigidTransform2d fromTranslation(Translation2d translation) { - return new RigidTransform2d(translation, new Rotation2d()); - } - - public static RigidTransform2d fromRotation(Rotation2d rotation) { - return new RigidTransform2d(new Translation2d(), rotation); - } - - /** - * Obtain a new RigidTransform2d from a (constant curvature) velocity. See: - * https://github.com/strasdat/Sophus/blob/master/sophus/se2.hpp - */ - public static RigidTransform2d fromVelocity(Delta delta) { - double sin_theta = Math.sin(delta.dtheta); - double cos_theta = Math.cos(delta.dtheta); - double s, c; - if (Math.abs(delta.dtheta) < kEps) { - s = 1.0 - 1.0 / 6.0 * delta.dtheta * delta.dtheta; - c = .5 * delta.dtheta; - } else { - s = sin_theta / delta.dtheta; - c = (1.0 - cos_theta) / delta.dtheta; - } - return new RigidTransform2d(new Translation2d(delta.dx * s - delta.dy * c, delta.dx * c + delta.dy * s), - new Rotation2d(cos_theta, sin_theta, false)); - } - - public Translation2d getTranslation() { - return translation_; - } - - public void setTranslation(Translation2d translation) { - translation_ = translation; - } - - public Rotation2d getRotation() { - return rotation_; - } - - public void setRotation(Rotation2d rotation) { - rotation_ = rotation; - } - - /** - * Transforming this RigidTransform2d means first translating by - * other.translation and then rotating by other.rotation - * - * @param other - * The other transform. - * @return This transform * other - */ - public RigidTransform2d transformBy(RigidTransform2d other) { - return new RigidTransform2d(translation_.translateBy(other.translation_.rotateBy(rotation_)), - rotation_.rotateBy(other.rotation_)); - } - - /** - * The inverse of this transform "undoes" the effect of translating by this - * transform. - * - * @return The opposite of this transform. - */ - public RigidTransform2d inverse() { - Rotation2d rotation_inverted = rotation_.inverse(); - return new RigidTransform2d(translation_.inverse().rotateBy(rotation_inverted), rotation_inverted); - } - - /** - * Do linear interpolation of this transform (there are more accurate ways - * using constant curvature, but this is good enough). - */ - @Override - public RigidTransform2d interpolate(RigidTransform2d other, double x) { - if (x <= 0) { - return new RigidTransform2d(this); - } else if (x >= 1) { - return new RigidTransform2d(other); - } - return new RigidTransform2d(translation_.interpolate(other.translation_, x), - rotation_.interpolate(other.rotation_, x)); - } - - @Override - public String toString() { - return "T:" + translation_.toString() + ", R:" + rotation_.toString(); - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java deleted file mode 100644 index 4da86a3..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java +++ /dev/null @@ -1,124 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.text.DecimalFormat; - -/** - * A rotation in a 2d coordinate frame represented a point on the unit circle - * (cosine and sine). - * - * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus) - */ -public class Rotation2d implements Interpolable { - protected static final double kEpsilon = 1E-9; - - protected double cos_angle_; - protected double sin_angle_; - - public Rotation2d() { - this(1, 0, false); - } - - public Rotation2d(double x, double y, boolean normalize) { - cos_angle_ = x; - sin_angle_ = y; - if (normalize) { - normalize(); - } - } - - public Rotation2d(Rotation2d other) { - cos_angle_ = other.cos_angle_; - sin_angle_ = other.sin_angle_; - } - - public static Rotation2d fromRadians(double angle_radians) { - return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false); - } - - public static Rotation2d fromDegrees(double angle_degrees) { - return fromRadians(Math.toRadians(angle_degrees)); - } - - /** - * From trig, we know that sin^2 + cos^2 == 1, but as we do math on this - * object we might accumulate rounding errors. Normalizing forces us to - * re-scale the sin and cos to reset rounding errors. - */ - public void normalize() { - double magnitude = Math.hypot(cos_angle_, sin_angle_); - if (magnitude > kEpsilon) { - sin_angle_ /= magnitude; - cos_angle_ /= magnitude; - } else { - sin_angle_ = 0; - cos_angle_ = 1; - } - } - - public double cos() { - return cos_angle_; - } - - public double sin() { - return sin_angle_; - } - - public double tan() { - if (cos_angle_ < kEpsilon) { - if (sin_angle_ >= 0.0) { - return Double.POSITIVE_INFINITY; - } else { - return Double.NEGATIVE_INFINITY; - } - } - return sin_angle_ / cos_angle_; - } - - public double getRadians() { - return Math.atan2(sin_angle_, cos_angle_); - } - - public double getDegrees() { - return Math.toDegrees(getRadians()); - } - - /** - * We can rotate this Rotation2d by adding together the effects of it and - * another rotation. - * - * @param other - * The other rotation. See: - * https://en.wikipedia.org/wiki/Rotation_matrix - * @return This rotation rotated by other. - */ - public Rotation2d rotateBy(Rotation2d other) { - return new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_, - cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true); - } - - /** - * The inverse of a Rotation2d "undoes" the effect of this rotation. - * - * @return The opposite of this rotation. - */ - public Rotation2d inverse() { - return new Rotation2d(cos_angle_, -sin_angle_, false); - } - - @Override - public Rotation2d interpolate(Rotation2d other, double x) { - if (x <= 0) { - return new Rotation2d(this); - } else if (x >= 1) { - return new Rotation2d(other); - } - double angle_diff = inverse().rotateBy(other).getRadians(); - return this.rotateBy(Rotation2d.fromRadians(angle_diff * x)); - } - - @Override - public String toString() { - final DecimalFormat fmt = new DecimalFormat("#0.000"); - return "(" + fmt.format(getDegrees()) + " deg)"; - } -} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java deleted file mode 100644 index 7bd6abd..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java +++ /dev/null @@ -1,114 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -public class SoftwarePIDController -{ - protected ArrayList motorControllers; - protected PIDParams pidParams; - protected boolean useGyroLock; - protected double targetGyroAngle; - protected MPSoftwareTurnType turnType; - - protected double minTurnOutput = 0.002; - protected double maxError; - protected double maxPrevError; - protected double prevError = 0.0; // the prior error (used to compute velocity) - protected double totalError = 0.0; // the sum of the errors for use in the integral calc - - public SoftwarePIDController( PIDParams pidParams, ArrayList motorControllers) - { - this.motorControllers = motorControllers; - setPID(pidParams); - } - - public void setPID(PIDParams pidParams) { - this.pidParams = pidParams; - } - - - public void setPIDTurnTarget(double targetAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) { - this.turnType = turnType; - this.targetGyroAngle = targetAngleDeg; - this.useGyroLock = true; - this.maxError = maxError; - this.maxPrevError = maxPrevError; - - // NOTE: below loop removed, as control mode is now changed as part of corresponding set() calls below - // for (CANTalonEncoder motorController : motorControllers) { - // motorController.changeControlMode(TalonControlMode.PercentVbus); - // } - - prevError = 0.0; - totalError = 0.0; - - } - - public boolean controlLoopUpdate() { - return controlLoopUpdate(0); - } - - public boolean controlLoopUpdate(double currentGyroAngle) { - // Calculate the motion profile feed forward and error feedback terms - double error = targetGyroAngle - currentGyroAngle; - double deltaLastError = (error - prevError); - - // Check if we are finished - if (Math.abs(error) < maxError && Math.abs(deltaLastError) < maxPrevError) { - return true; - } - - - if (Math.abs(targetGyroAngle - currentGyroAngle) < pidParams.iZone) { - totalError += error; - } - else { - totalError = 0; - } - - double output = pidParams.kP * error + pidParams.kI * totalError + pidParams.kD * deltaLastError; - double turnBoost = output < 0 ? -minTurnOutput : minTurnOutput; - output += turnBoost; - - prevError = error; - - // Update the controllers set point. - if (turnType == MPSoftwareTurnType.TANK) { - for (CANTalonEncoder motorController : motorControllers) { - //motorController.set(output); - motorController.set(ControlMode.PercentOutput, output); - } - } - else if (turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); - } - else { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - } - } - else if (turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { - for (CANTalonEncoder motorController : motorControllers) { - if (motorController.isRight()) { - //motorController.set(2.0 * output); - motorController.set(ControlMode.PercentOutput, 2.0 * output); - } - else { - //motorController.set(0); - motorController.set(ControlMode.PercentOutput, 0); - } - } - } - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java deleted file mode 100644 index 383acd7..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java +++ /dev/null @@ -1,85 +0,0 @@ - -package org.usfirst.frc4388.utility; - -import java.util.ArrayList; - -import org.usfirst.frc4388.robot.Robot; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -import edu.wpi.first.wpilibj.Timer; - -public class SoftwarePIDPositionController -{ - //protected ArrayList motorControllers; - protected WPI_TalonSRX motorController; - protected PIDParams pidParams; - - protected PIDParams PValue; - - protected double targetEncoderPosition; - - protected double minTurnOutput = 0.002; - protected double maxError; - protected double minError; - protected double maxPrevError; ///?? - protected double prevError = 0.0; // the prior error (used to compute velocity) - protected double totalError = 0.0; // the sum of the errors for use in the integral calc - - public SoftwarePIDPositionController(PIDParams PValue, WPI_TalonSRX elevatorLeft) - { - this.motorController = elevatorLeft; - setP(PValue); - } - - public void setP(PIDParams PValue) - { - this.PValue = PValue; - } - - public void setPIDPositionTarget(double targetPosition, double maxError, double minError) - { - this.targetEncoderPosition = targetPosition; - - this.maxError = maxError; - this.minError = minError; - //this.maxPrevError = maxPrevError; - - prevError = 0.0; - totalError = 0.0; - } - - public boolean controlLoopUpdate() - { - return controlLoopUpdate(0); - } - - public boolean controlLoopUpdate(double currentEncoderPosition) - { - // Calculate the motion profile feed forward and error feedback terms - double error = targetEncoderPosition - currentEncoderPosition; - //double deltaLastError = (error - prevError); - - //Updating the error - //totalError += error; - - // Check if we are finished - if (Math.abs(error) < maxError && Math.abs(error) > minError) - { - //Robot.elevator.holdInPos(); - - return true; - } - - double output = pidParams.kP * error; // + pidParams.kI * totalError + pidParams.kD * deltaLastError; - - prevError = error; - - // Update the controllers set point. - motorController.set(ControlMode.PercentOutput, output); - - return false; - } -} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java deleted file mode 100644 index 05417a8..0000000 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java +++ /dev/null @@ -1,104 +0,0 @@ -package org.usfirst.frc4388.utility; - -import java.text.DecimalFormat; - -/** - * A translation in a 2d coordinate frame. Translations are simply shifts in an - * (x, y) plane. - */ -public class Translation2d implements Interpolable { - protected double x_; - protected double y_; - - public Translation2d() { - x_ = 0; - y_ = 0; - } - - public Translation2d(double x, double y) { - x_ = x; - y_ = y; - } - - public Translation2d(Translation2d other) { - x_ = other.x_; - y_ = other.y_; - } - - /** - * The "norm" of a transform is the Euclidean distance in x and y. - * - * @return Sqrt(x^2 + y^2) - */ - public double norm() { - return Math.hypot(x_, y_); - } - - public double getX() { - return x_; - } - - public double getY() { - return y_; - } - - public void setX(double x) { - x_ = x; - } - - public void setY(double y) { - y_ = y; - } - - /** - * We can compose Translation2d's by adding together the x and y shifts. - * - * @param other - * The other translation to add. - * @return The combined effect of translating by this object and the other. - */ - public Translation2d translateBy(Translation2d other) { - return new Translation2d(x_ + other.x_, y_ + other.y_); - } - - /** - * We can also rotate Translation2d's. See: - * https://en.wikipedia.org/wiki/Rotation_matrix - * - * @param rotation - * The rotation to apply. - * @return This translation rotated by rotation. - */ - public Translation2d rotateBy(Rotation2d rotation) { - return new Translation2d(x_ * rotation.cos() - y_ * rotation.sin(), x_ * rotation.sin() + y_ * rotation.cos()); - } - - /** - * The inverse simply means a Translation2d that "undoes" this object. - * - * @return Translation by -x and -y. - */ - public Translation2d inverse() { - return new Translation2d(-x_, -y_); - } - - @Override - public Translation2d interpolate(Translation2d other, double x) { - if (x <= 0) { - return new Translation2d(this); - } else if (x >= 1) { - return new Translation2d(other); - } - return extrapolate(other, x); - } - - public Translation2d extrapolate(Translation2d other, double x) { - return new Translation2d(x * (other.x_ - x_) + x_, x * (other.y_ - y_) + y_); - } - - @Override - public String toString() { - final DecimalFormat fmt = new DecimalFormat("#0.000"); - return "(" + fmt.format(x_) + "," + fmt.format(y_) + ")"; - } -} diff --git a/2019robot/vendordeps/PathfinderOLD.json b/2019robot/vendordeps/PathfinderOLD.json deleted file mode 100644 index 2551e4d..0000000 --- a/2019robot/vendordeps/PathfinderOLD.json +++ /dev/null @@ -1,85 +0,0 @@ -{ - "fileName": "PathfinderOLD.json", - "name": "PathfinderOld", - "version": "2019.1.11", - "uuid": "7194a2d4-2860-4bcc-86c0-97879737d875", - "mavenUrls": [ - "https://dev.imjac.in/maven" - ], - "jsonUrl": "https://dev.imjac.in/maven/jaci/pathfinder/PathfinderOLD-latest.json", - "cppDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-Core", - "version": "2019.1.11", - "libName": "pathfinder", - "configuration": "native_pathfinder_old", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-FRCSupport", - "version": "2019.1.11", - "libName": "pathfinder_frc", - "configuration": "native_pathfinder_old", - "headerClassifier": "headers", - "binaryPlatforms": [] - } - ], - "javaDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-Java", - "version": "2019.1.11" - }, - { - "groupId": "jaci.jniloader", - "artifactId": "JNILoader", - "version": "1.0.1" - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-FRCSupport", - "version": "2019.1.11" - } - ], - "jniDependencies": [ - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-JNI", - "version": "2019.1.11", - "isJar": true, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - }, - { - "groupId": "jaci.pathfinder", - "artifactId": "Pathfinder-CoreJNI", - "version": "2019.1.11", - "isJar": true, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxx86-64", - "windowsx86-64", - "osxx86-64", - "linuxathena", - "linuxraspbian" - ] - } - ] -} \ No newline at end of file diff --git a/2019robot/vendordeps/Phoenix.json b/2019robot/vendordeps/Phoenix.json deleted file mode 100644 index d4da1ce..0000000 --- a/2019robot/vendordeps/Phoenix.json +++ /dev/null @@ -1,87 +0,0 @@ -{ - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix", - "version": "5.12.0", - "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.12.0" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.12.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.12.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.12.0", - "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.12.0", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.12.0", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "core", - "version": "5.12.0", - "libName": "CTRE_PhoenixCore", - "headerClassifier": "headers" - } - ] -} \ No newline at end of file diff --git a/2019robot/vendordeps/REVRobotics.json b/2019robot/vendordeps/REVRobotics.json deleted file mode 100644 index d997798..0000000 --- a/2019robot/vendordeps/REVRobotics.json +++ /dev/null @@ -1,32 +0,0 @@ -{ - "fileName": "REVRobotics.json", - "name": "REVRobotics", - "version": "1.0.26", - "uuid": "c16ed09f-23df-4beb-87e8-460bd7fa9924", - "mavenUrls": [ - "http://www.revrobotics.com/content/sw/max/sdk/maven/" - ], - "jsonUrl": "http://www.revrobotics.com/content/sw/max/sdk/REVRobotics.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "SparkMax-java", - "version": "1.0.26" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "SparkMax-cpp", - "version": "1.0.26", - "libName": "libSparkMax", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - } - ] -} \ No newline at end of file diff --git a/2019robot/vendordeps/cw.json b/2019robot/vendordeps/cw.json deleted file mode 100644 index 2c3329e..0000000 --- a/2019robot/vendordeps/cw.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "fileName": "cw.json", - "name": "cw", - "version": "3.1.344", - "uuid":"13", - "mavenUrls": [ - "https://mvnrepository.com" - ], - "jsonUrl": "file:///C:/dev/cw.json", - "javaDependencies": [ - { - "groupId": "com.googlecode.json-simple", - "artifactId": "json-simple", - "version": "1.1.1" - } - ], - "jniDependencies": [], - "cppDependencies": [ - - ] -} \ No newline at end of file diff --git a/2019robot/vendordeps/navx_frc.json b/2019robot/vendordeps/navx_frc.json deleted file mode 100644 index 80defba..0000000 --- a/2019robot/vendordeps/navx_frc.json +++ /dev/null @@ -1,33 +0,0 @@ -{ - "fileName": "navx_frc.json", - "name": "KauaiLabs_navX_FRC", - "version": "3.1.344", - "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", - "mavenUrls": [ - "https://repo1.maven.org/maven2/" - ], - "jsonUrl": "https://www.kauailabs.com/dist/frc/2019/navx_frc.json", - "javaDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-java", - "version": "3.1.344" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.kauailabs.navx.frc", - "artifactId": "navx-cpp", - "version": "3.1.344", - "headerClassifier": "headers", - "sourcesClassifier": "sources", - "sharedLibrary": false, - "libName": "navx_frc", - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena" - ] - } - ] -} \ No newline at end of file