Clear Master

2019robot/.gitignore

@@ -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

2019robot/.vscode/launch.json

@@ -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,
-    }
-  ]
-}

2019robot/.vscode/settings.json

@@ -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
-}

2019robot/.wpilib/wpilib_preferences.json

@@ -1,6 +0,0 @@
-{
-    "enableCppIntellisense": false,
-    "currentLanguage": "java",
-    "projectYear": "2019",
-    "teamNumber": 4388
-}

2019robot/build.gradle

@@ -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)
-}

2019robot/gradle/wrapper/gradle-wrapper.properties

@@ -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

2019robot/gradlew

@@ -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" "$@"

2019robot/gradlew.bat

@@ -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

2019robot/settings.gradle

@@ -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
-        }
-    }
-}

2019robot/src/main/deploy/example.txt

@@ -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.

2019robot/src/main/java/buttons/XBoxTriggerButton.java

@@ -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;
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java

@@ -1,12 +0,0 @@
-package org.usfirst.frc4388.controller;
-
-public interface IHandController {
-
-	public double getLeftXAxis();
-
-	public double getLeftYAxis();
-
-	public double getRightXAxis();
-	
-	public double getRightYAxis();
-}

2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java

@@ -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);
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java

@@ -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
-
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java

@@ -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.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java

@@ -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();
-		}
-	}
-

2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java

@@ -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<OperationMode> operationModeChooser;
-	private SendableChooser<Command> RRautonTaskChooser;
-	private SendableChooser<Command> RLautonTaskChooser;
-	private SendableChooser<Command> LRautonTaskChooser;
-	private SendableChooser<Command> 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<OperationMode>();
-	    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);
-   }
-
-}
-

2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java

@@ -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;
-
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmBasic.java

@@ -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();
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetSpeed.java

@@ -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() {
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java

@@ -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();
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java

@@ -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() {
-			
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java

@@ -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();
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java

@@ -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() {
-			
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java

@@ -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();
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java

@@ -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();
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java

@@ -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();
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java

@@ -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();
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java

@@ -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() {
-			
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java

@@ -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<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();	
-    
-    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");
-			}
-		}
-	}
-
-	
-}
-

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java

@@ -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
-			
-		}
-
-}
-

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java

@@ -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<CANTalonEncoder> motorControllers = new ArrayList<CANTalonEncoder>();	
-
-	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<String> 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 {
-			
-		}
-	}	
-	
-}

2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java

@@ -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() {
-    }
-}
-

2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java

@@ -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<CANTalonEncoder> motorControllers;	
-	protected long periodMs;
-	protected PIDParams pidParams;	
-	protected double startGyroAngle;
-	protected double targetGyroAngle;
-	protected double trackDistance;
-	
-	public AdaptivePurePursuitController(long periodMs, PIDParams pidParams, ArrayList<CANTalonEncoder> 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> 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<String> 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<Circle> 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));
-    }
-
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java

@@ -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));
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java

@@ -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"
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java

@@ -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<String, Boolean> modifiedKeys = new HashMap<String, Boolean>();
-
-    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<Constant> getConstants() {
-        List<Constant> constants = (List<Constant>) 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<Constant> getAllConstants() {
-        Field[] declaredFields = this.getClass().getDeclaredFields();
-        List<Constant> constants = new ArrayList<Constant>(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();
-        }
-    }
-
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java

@@ -1,7 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-public interface ControlLoopable 
-{
-	public void controlLoopUpdate();
-	public void setPeriodMs(long periodMs);
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java

@@ -1,79 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-import java.util.Timer;
-import java.util.TimerTask;
-import java.util.Vector;
-
-/**
- * ControlLooper.java
- * <p>
- * 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<ControlLoopable> loopables = new Vector<ControlLoopable>();
-	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);
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/FalconLinePlot.java

@@ -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<xyNode> 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<xyNode>();
-    	
-    	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<y.length; i++)
-    		Data.y[i] = y[i];
-    	
-    	//if X is not null, copy x
-    	if(x != null)
-    	{
-        	//cant add x, and y data unless all other data has x and y data
-        	
-        	for(xyNode data: link)
-        		if(data.x == null)
-        			throw new Error ("All previous chart series need to have both X and Y data arrays");
-    		
-    		if(x.length != y.length)
-    			throw new Error("X dimension must match Y dimension");
-    		
-    		Data.x = new double[x.length];
-    		
-    		for(int i=0; i<x.length; i++)
-        		Data.x[i] = x[i];
-    		
-    	}
-    	link.add(Data);
-    }
- 
-    /**
-     * Main method which paints the panel and shows the figure.
-     * 
-     */
-    @Override
-    protected void paintComponent(Graphics g) {
-        super.paintComponent(g);
-        Graphics2D g2 =  (Graphics2D)g;
-        g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
-        
-        int w = getWidth();
-        int h = getHeight();
-    
-        // Draw X and Y lines axis.
-        Line2D yaxis = new Line2D.Double(xPAD, yPAD, xPAD, h-yPAD);
-        Line2D.Double xaxis = new Line2D.Double(xPAD, h-yPAD, w-xPAD, h-yPAD);
-        g2.draw(yaxis); 
-        g2.draw(xaxis);
-        
-        //find Max Y limits
-        getMinMax(link);
-        
-        //draw ticks
-        drawYTickRange(g2, yaxis, 15, yMax, yMin);
-        drawXTickRange(g2, xaxis, 15, xMax, xMin);
-        
-        //plot all data
-        plot(g2);
-        
-        //draw x and y labels
-        setXLabel(g2, xLabel);
-        setYLabel(g2, yLabel);
-        setTitle(g2, titleLabel);
-        
-    }
-    
-    void setXTic(double lowerBound, double upperBound, double stepSize)
-    {
-    	this.userSetXTic = true;
-    	
-    	this.upperXtic=upperBound;
-    	this.lowerXtic=lowerBound;
-    	this.xTicStepSize = stepSize;
-    }
-    
-    public void setYTic(double lowerBound, double upperBound, double stepSize)
-    {
-    	this.userSetYTic = true;
-    	
-    	this.upperYtic=upperBound;
-    	this.lowerYtic=lowerBound;
-    	this.yTicStepSize = stepSize;
-    }
-    
-    private void plot(Graphics2D g2)
-    {
-    	
-    	int w = super.getWidth();
-    	int h = super.getHeight();
-    	
-    	Color tempC = g2.getColor();
-    	
-    	//loop through list and plot each
-    	for(int i=0; i<link.size(); i++)
-    	{
-	        // Draw lines.
-	        double xScale = (double)(w - 2*xPAD)/(upperXtic-lowerXtic);
-	        double yScale = (double)(h - 2*yPAD)/(upperYtic-lowerYtic);
-	        
-	        for(int j = 0; j < link.get(i).y.length-1; j++) 
-	        {
-	        	double x1;
-	        	double x2;
-	        	
-	            if(link.get(i).x==null)
-	            {
-	            	x1 = xPAD + j*xScale;
-	            	x2 = xPAD + (j+1)*xScale;
-	            }
-	            else
-	            {
-	            	x1 = xPAD + xScale*link.get(i).x[j] + lowerXtic*xScale;
-	            	x2 = xPAD + xScale*link.get(i).x[j+1] + lowerXtic*xScale;
-	            }
-	            
-	            double y1 = h - yPAD - yScale*link.get(i).y[j] + lowerYtic*yScale;
-	            
-	            double y2 = h - yPAD - yScale*link.get(i).y[j+1] + lowerYtic*yScale;
-	            g2.setPaint(link.get(i).lineColor);
-	            g2.draw(new Line2D.Double(x1, y1, x2, y2));
-	            
-	            if(link.get(i).lineMarker)
-	            {
-	            	g2.setPaint(link.get(i).markerColor);
-	            	g2.fill(new Ellipse2D.Double(x1-2, y1-2, 4, 4));
-	            	g2.fill(new Ellipse2D.Double(x2-2, y2-2, 4, 4));
-	            }
-	      
-	        }
-	        	        	
-    	}
-    	
-    	g2.setColor(tempC);
-    }
-    
-    /**
-     * need to optimize for loops
-     * @param list
-     */
-    private void getMinMax(LinkedList<xyNode> list)
-    {
-    	for(xyNode node: list)
-    	{
-    		double tempYMax = getMax(node.y);
-    		double tempYMin = getMin(node.y);
-    		
-    		if(tempYMin<yMin)
-    			yMin = tempYMin;
-    		
-    		if(tempYMax>yMax)
-    			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(tempXMin<xMin)
-        			xMin = tempXMin;
-        		
-        		if(tempXMax>xMax)
-        			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<line.length; i++)
-    	{
-    		
-    		int width = fm.stringWidth(line[i]);
-            g2.drawString(line[i], getWidth()/2-(width/2),  height);
-            height +=fm.getHeight();
-            
-    	}
-   
-    }
-
-    
-    public void yGridOn()
-    {
-    	yGrid=true;
-    	//super.repaint();
-    }
-    
-    public void yGridOff()
-    {
-    	yGrid=false;
-    	//super.repaint();
-    }
-    
-    public void xGridOn()
-    {
-    	xGrid=true;
-    	//super.repaint();
-    }
-    
-    public void xGridOff()
-    {
-    	xGrid=false;
-    	//super.repaint();
-    }
-    
-    private void drawYTickRange(Graphics2D g2, Line2D yaxis, int tickCount, double Max, double Min)
-    {
-    	if(!userSetYTic)
-    	{
-    	double range = Max - Min;
-    	
-    	
-    	//calculate max Y and min Y tic Range
-    	double unroundedTickSize = range/(tickCount-1);
-    	double x = Math.ceil(Math.log10(unroundedTickSize)-1);
-    	double pow10x = Math.pow(10, x);
-    	yTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
-    	
-    	//determine min and max tick label 
-        if(Min<0)
-        	lowerYtic = yTicStepSize * Math.floor(Min/yTicStepSize);
-        else
-        	lowerYtic = yTicStepSize * Math.ceil(Min/yTicStepSize);
-        
-        if(Max<0)
-        	upperYtic = yTicStepSize * Math.floor(1+Max/yTicStepSize);
-        else
-        	upperYtic = yTicStepSize * Math.ceil(1+Max/yTicStepSize);
-        
-    	}
-    	
-        
-        double x0 = yaxis.getX1();
-        double y0 = yaxis.getY1();
-        double xf = yaxis.getX2();
-        double yf = yaxis.getY2();
-        
-        //calculate stepsize between ticks and length of Y axis using distance formula
-        int roundedTicks = (int) ((upperYtic - lowerYtic) / yTicStepSize);
-        double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
-        
-        double upper = upperYtic;
-        for (int i = 0; i<=roundedTicks; i++)
-        {
-        	double newY = y0;
-        	
-        	//calculate width of number for proper drawing
-        	String number = new DecimalFormat("#.#").format(upper);
-        	FontMetrics fm = getFontMetrics(getFont());
-        	int width = fm.stringWidth(number);
-   
-        	g2.draw(new Line2D.Double(x0,newY, x0-10,newY));
-        	g2.drawString(number, (float)x0-15-width, (float)newY+5); 
-        	 
-        	//add grid lines to chart
-        	if(yGrid && i!=roundedTicks)
-        	{
-
-        		Stroke tempS = g2.getStroke();
-        		Color tempC = g2.getColor();
-        		
-        		g2.setColor (Color.lightGray);
-          	    g2.setStroke (new BasicStroke(
-          	      1f, 
-          	      BasicStroke.CAP_ROUND, 
-          	      BasicStroke.JOIN_ROUND, 
-          	      1f, 
-          	      new float[] {5f}, 
-          	      0f));
-          	    
-          	    g2.draw(new Line2D.Double(xPAD, newY, getWidth()-xPAD, newY));
-        		
-          	    g2.setColor(tempC);
-          	    g2.setStroke(tempS);
-        		
-        	}
-        	
-        	  upper = upper - yTicStepSize;
-        	y0 = newY + distance;
-        	
-        }
-    }
-    
-    private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min)
-    {
-    	drawXTickRange(g2, xaxis, tickCount, Max, Min, 1);
-    }
-    
-    private void drawXTickRange(Graphics2D g2, Line2D xaxis, int tickCount, double Max, double Min, double skip)
-    {
-    	if(!userSetXTic)
-    	{
-    	double range = Max - Min;
-    	
-    	//calculate max Y and min Y tic Range
-    	double unroundedTickSize = range/(tickCount-1);
-    	double x = Math.ceil(Math.log10(unroundedTickSize)-1);
-    	double pow10x = Math.pow(10, x);
-    	xTicStepSize = Math.ceil(unroundedTickSize / pow10x) * pow10x;
-    	
-    	//determine min and max tick label 
-        if(Min<0)
-        	lowerXtic = xTicStepSize * Math.floor(Min/xTicStepSize);
-        else
-        	lowerXtic = xTicStepSize * Math.ceil(Min/xTicStepSize);
-        
-        if(Max<0)
-        	upperXtic = xTicStepSize * Math.floor(1+Max/xTicStepSize);
-        else
-        	upperXtic = xTicStepSize * Math.ceil(1+Max/xTicStepSize);
-    	}
-        
-        
-        double x0 = xaxis.getX1();
-        double y0 = xaxis.getY1();
-        double xf = xaxis.getX2();
-        double yf = xaxis.getY2();
-        
-        //calculate stepsize between ticks and length of Y axis using distance formula
-        int roundedTicks = (int) ((upperXtic - lowerXtic) / xTicStepSize);
-        
-        
-        double distance = Math.sqrt(Math.pow((xf-x0), 2)+Math.pow((yf-y0), 2)) / roundedTicks;
-        
-        double lower = lowerXtic;
-        for (int i = 0; i<=roundedTicks; i++)
-        {
-        	double newX = x0;
-        	
-        	//calculate width of number for proper drawing
-        	String number = new DecimalFormat("#.#").format(lower);
-        	FontMetrics fm = getFontMetrics( getFont() );
-        	int width = fm.stringWidth(number);
-        	
-   
-        	g2.draw(new Line2D.Double(newX,yf, newX,yf+10));
-        	
-        	//dont label every x tic to prevent clutter
-        	if(i%skip==0)
-       		g2.drawString(number, (float)(newX-(width/2.0)), (float)yf+25); 
-        	
-        	//add grid lines to chart
-        	if(xGrid && i!=0)
-        	{
-        		Stroke tempS = g2.getStroke();
-        		Color tempC = g2.getColor();
-        		
-        		g2.setColor (Color.lightGray);
-          	    g2.setStroke (new BasicStroke(
-          	      1f, 
-          	      BasicStroke.CAP_ROUND, 
-          	      BasicStroke.JOIN_ROUND, 
-          	      1f, 
-          	      new float[] {5f}, 
-          	      0f));
-          	    
-          	    g2.draw(new Line2D.Double(newX, yPAD, newX, getHeight()-yPAD));
-        		
-          	    g2.setColor(tempC);
-          	    g2.setStroke(tempS);
-        		
-        	}
-        	
-        	 
-        	lower = lower + xTicStepSize;
-        	x0 = newX + distance;
-        }
-    }
-    
-    public void updateData(int series, double[][] data)
-    {
-    	//add Data to link list
-    	addData(data,null,null);
-    	
-    	//copy data from new to old and line styles from list to new list.
-    	
-    	link.get(series).x = link.getLast().x.clone();
-    	link.get(series).y = link.getLast().y.clone();
-    	
-    	//remove last data
-    	link.removeLast();
-    
-    	
-    	
-    }
-    
-	public static double[] getXVector(double[][] arr)
-	{
-		double[] temp = new double[arr.length];
-
-		for(int i=0; i<temp.length; i++)
-			temp[i] = arr[i][0];
-
-		return temp;		
-	}
-
-	public static double[] getYVector(double[][] arr)
-	{
-		double[] temp = new double[arr.length];
-
-		for(int i=0; i<temp.length; i++)
-			temp[i] = arr[i][1];
-
-		return temp;		
-	}
-	
-	/**********Class for Linked List************/
-    private class xyNode
-    {
-    	double[] x;
-    	double[] y;
-    	Color lineColor;
-    	
-    	boolean lineMarker;
-    	Color markerColor;
-    	
-    	public xyNode()
-    	{
-    		x=null;
-    		y=null;
-    		
-    		lineMarker = false;
-    	}
-    }
-	
-    /****Methods to Support Right Click Menu****/
-    @Override
-	public void lostOwnership(Clipboard    clip, Transferable transferable) 
-	{
-		//We must keep the object we placed on the system clipboard
-		//until this method is called.
-	}
-
-    
-    
-    private void menu(JFrame g, final FalconLinePlot p )
-    {
-
-    	g.addMouseListener(new PopupTriggerListener());
-    	
-    	JMenuItem item = new JMenuItem("Copy Figure");
-
-        item.addActionListener(new ActionListener() 
-        {
-          public void actionPerformed(ActionEvent e) 
-          {
-            
-            BufferedImage i = new BufferedImage(p.getSize().width, p.getSize().height,BufferedImage.TRANSLUCENT);
-            p.setOpaque(false);
-            p.paint(i.createGraphics()); 
-            TransferableImage trans = new TransferableImage( i );
-			Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
-			c.setContents( trans, p);
-          }
-        });
-        
-        menu.add(item);
-    	
-        item = new JMenuItem("Desktop ScreenShot");
-        item.addActionListener(new ActionListener() 
-        {
-          public void actionPerformed(ActionEvent e) 
-          {
-            System.out.println("Copy files to clipboard");
-            
-            try {
-                Robot robot = new Robot();
-                Dimension screenSize  = Toolkit.getDefaultToolkit().getScreenSize();
-                Rectangle screen = new Rectangle( screenSize );
-                BufferedImage i = robot.createScreenCapture( screen );
-                TransferableImage trans = new TransferableImage( i );
-                Clipboard c = Toolkit.getDefaultToolkit().getSystemClipboard();
-                c.setContents( trans, p);
-            }
-            catch ( AWTException x ) {
-                x.printStackTrace();
-                System.exit( 1 );
-            }
-            
-          }
-        });
-        
-        menu.add(item);
-    }
-
-    class PopupTriggerListener extends MouseAdapter {
-    	public void mousePressed(MouseEvent ev) {
-    		if (ev.isPopupTrigger()) {
-    			menu.show(ev.getComponent(), ev.getX(), ev.getY());
-    		}
-    	}
-
-    	public void mouseReleased(MouseEvent ev) {
-    		if (ev.isPopupTrigger()) {
-    			menu.show(ev.getComponent(), ev.getX(), ev.getY());
-    		}
-    	}
-
-    	public void mouseClicked(MouseEvent ev) {
-
-    	}
-    }
-
-    private class TransferableImage implements Transferable {
-
-        Image i;
-
-        public TransferableImage( Image i ) {
-            this.i = i;
-        }
-
-        public Object getTransferData( DataFlavor flavor )
-        throws UnsupportedFlavorException, IOException {
-            if ( flavor.equals( DataFlavor.imageFlavor ) && i != null ) {
-                return i;
-            }
-            else {
-                throw new UnsupportedFlavorException( flavor );
-            }
-        }
-
-        public DataFlavor[] getTransferDataFlavors() {
-            DataFlavor[] flavors = new DataFlavor[ 1 ];
-            flavors[ 0 ] = DataFlavor.imageFlavor;
-            return flavors;
-        }
-
-        public boolean isDataFlavorSupported( DataFlavor flavor ) {
-            DataFlavor[] flavors = getTransferDataFlavors();
-            for ( int i = 0; i < flavors.length; i++ ) {
-                if ( flavor.equals( flavors[ i ] ) ) {
-                    return true;
-                }
-            }
-
-            return false;
-        }
-    }
-    
-    /******TEST MAIN METHOD*******/
-    public static void main(String[] args) {
-    	
-  	  double[] data = {
-  		        -235, 14, 18, 03, 60, 150, 74, 87, 54, 77,
-  		        61, 55, 48, 60, 49, 36, 38, 27, 20, 18,5
-  		    };
-  	  
-  	  double[] data2 = {
-		        -4, 124, 128, 33, -1, 1, 74, 87, 54, 77,
-		        61, 55, 48, 60, 40, 36, 38, 27, 20, 18,5
-		    };
-  	  
-  	  double[] test = {0.1, 0.3, 0.5, 0.7, 0.9, 1.1, 1.3, 1.5, 1.7, 2.0,
-  			  2.2,2.4,2.6,2.8,3.0,3.2,3.4,3.6,3.8,4.0,4.2
-  	  };
-  	
-  	
-  	
-  	FalconLinePlot fig2 = new FalconLinePlot(data,Color.red, Color.blue);
-  	
-  	fig2.yGridOn();
-  	fig2.xGridOn();
-  	fig2.setYLabel("This is a new");
-  	
-  	fig2.addData(data2, Color.blue);
-  	
-  	FalconLinePlot fig1 =  new FalconLinePlot(test,data);
-  	
-  }
-    
-    
-}
-   

2019robot/src/main/java/org/usfirst/frc4388/utility/Interpolable.java

@@ -1,27 +0,0 @@
-package org.usfirst.frc4388.utility;
-
-/**
- * Interpolable is an interface used by an Interpolating Tree as the Value type.
- * Given two end points and an interpolation parameter on [0, 1], it calculates
- * a new Interpolable representing the interpolated value.
- * 
- * @param <T>
- *            The Type of Interpolable
- * @see InterpolatingTreeMap
- */
-public interface Interpolable<T> {
-    /**
-     * 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<T> The estimated average between the surrounding
-     *         data
-     */
-    public T interpolate(T other, double x);
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/InterpolatingTreeMap.java

@@ -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 <K>
- *            The type of the key (must implement InverseInterpolable)
- * @param <V>
- *            The type of the value (must implement Interpolable)
- */
-public class InterpolatingTreeMap<K extends InverseInterpolable<K> & Comparable<K>, V extends Interpolable<V>>
-        extends TreeMap<K, V> {
-    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<? extends K, ? extends V> 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;
-        }
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/InverseInterpolable.java

@@ -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 <T>
- *            The Type of InverseInterpolable
- * @see InterpolatingTreeMap
- */
-public interface InverseInterpolable<T> {
-    /**
-     * 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);
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/Kinematics.java

@@ -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);
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java

@@ -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<CANTalonEncoder> 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<CANTalonEncoder> 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;
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/MPSoftwarePIDController.java

@@ -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<CANTalonEncoder> 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<CANTalonEncoder> 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;
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDController.java

@@ -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<CANTalonEncoder> 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<CANTalonEncoder> 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;
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathController.java

@@ -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<CANTalonEncoder> 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<CANTalonEncoder> 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;
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/MPTalonPIDPathVelocityController.java

@@ -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<CANTalonEncoder> 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<CANTalonEncoder> 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;
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfileBoxCar.java

@@ -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");
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/MotionProfilePoint.java

@@ -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;
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/PIDParams.java

@@ -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;
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/Path.java

@@ -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<Waypoint> mWaypoints;
-    protected List<PathSegment> mSegments;
-    protected Set<String> 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<String> 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<Waypoint> waypoints) {
-        mMarkersCrossed = new HashSet<String>();
-        mWaypoints = waypoints;
-        mSegments = new ArrayList<PathSegment>();
-        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<PathSegment> 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<String> 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<Translation2d> 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<Translation2d> 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<Translation2d> 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<Waypoint> 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<Waypoint> 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_);
-        }
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java

@@ -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");
-  }
-
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/PathSegment.java

@@ -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;
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/RigidTransform2d.java

@@ -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<RigidTransform2d> {
-    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();
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/Rotation2d.java

@@ -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<Rotation2d> {
-    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)";
-    }
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDController.java

@@ -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<CANTalonEncoder> 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<CANTalonEncoder> 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;
-	}	
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java

@@ -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<CANTalonEncoder> 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;
-	}
-}

2019robot/src/main/java/org/usfirst/frc4388/utility/Translation2d.java

@@ -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<Translation2d> {
-    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_) + ")";
-    }
-}

2019robot/vendordeps/PathfinderOLD.json

@@ -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"
-            ]
-        }
-    ]
-}

2019robot/vendordeps/Phoenix.json

@@ -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"
-        }
-    ]
-}

2019robot/vendordeps/REVRobotics.json

@@ -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"
-            ]
-        }
-    ]
-}

2019robot/vendordeps/cw.json

@@ -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": [
-       
-    ]
-}

2019robot/vendordeps/navx_frc.json

@@ -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"
-            ]
-        }
-    ]
-}