diff --git a/2019robot/.gitignore b/2019robot/.gitignore new file mode 100644 index 0000000..61f25ce --- /dev/null +++ b/2019robot/.gitignore @@ -0,0 +1,160 @@ +# Created by https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode + +### C++ ### +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +### Java ### +# Compiled class file +*.class + +# Log file +*.log + +# BlueJ files +*.ctxt + +# Mobile Tools for Java (J2ME) +.mtj.tmp/ + +# Package Files # +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +# virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + +### Linux ### +*~ + +# temporary files which can be created if a process still has a handle open of a deleted file +.fuse_hidden* + +# KDE directory preferences +.directory + +# Linux trash folder which might appear on any partition or disk +.Trash-* + +# .nfs files are created when an open file is removed but is still being accessed +.nfs* + +### macOS ### +# General +.DS_Store +.AppleDouble +.LSOverride + +# Icon must end with two \r +Icon + +# Thumbnails +._* + +# Files that might appear in the root of a volume +.DocumentRevisions-V100 +.fseventsd +.Spotlight-V100 +.TemporaryItems +.Trashes +.VolumeIcon.icns +.com.apple.timemachine.donotpresent + +# Directories potentially created on remote AFP share +.AppleDB +.AppleDesktop +Network Trash Folder +Temporary Items +.apdisk + +### VisualStudioCode ### +.vscode/* +!.vscode/settings.json +!.vscode/tasks.json +!.vscode/launch.json +!.vscode/extensions.json + +### Windows ### +# Windows thumbnail cache files +Thumbs.db +ehthumbs.db +ehthumbs_vista.db + +# Dump file +*.stackdump + +# Folder config file +[Dd]esktop.ini + +# Recycle Bin used on file shares +$RECYCLE.BIN/ + +# Windows Installer files +*.cab +*.msi +*.msix +*.msm +*.msp + +# Windows shortcuts +*.lnk + +### Gradle ### +.gradle +/build/ + +# Ignore Gradle GUI config +gradle-app.setting + +# Avoid ignoring Gradle wrapper jar file (.jar files are usually ignored) +!gradle-wrapper.jar + +# Cache of project +.gradletasknamecache + +# # Work around https://youtrack.jetbrains.com/issue/IDEA-116898 +# gradle/wrapper/gradle-wrapper.properties + +# # VS Code Specific Java Settings +.classpath +.project +.settings/ +bin/ + + +# End of https://www.gitignore.io/api/c++,java,linux,macos,gradle,windows,visualstudiocode diff --git a/2019robot/.vscode/launch.json b/2019robot/.vscode/launch.json new file mode 100644 index 0000000..c9c9713 --- /dev/null +++ b/2019robot/.vscode/launch.json @@ -0,0 +1,21 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + + { + "type": "wpilib", + "name": "WPILib Desktop Debug", + "request": "launch", + "desktop": true, + }, + { + "type": "wpilib", + "name": "WPILib roboRIO Debug", + "request": "launch", + "desktop": false, + } + ] +} diff --git a/2019robot/.vscode/settings.json b/2019robot/.vscode/settings.json new file mode 100644 index 0000000..860e319 --- /dev/null +++ b/2019robot/.vscode/settings.json @@ -0,0 +1,14 @@ +{ + "java.configuration.updateBuildConfiguration": "automatic", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + ".classpath": true, + ".project": true + }, + "wpilib.online": true +} diff --git a/2019robot/.wpilib/wpilib_preferences.json b/2019robot/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..95c6671 --- /dev/null +++ b/2019robot/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2019", + "teamNumber": 4388 +} \ No newline at end of file diff --git a/2019robot/build.gradle b/2019robot/build.gradle new file mode 100644 index 0000000..f15e84c --- /dev/null +++ b/2019robot/build.gradle @@ -0,0 +1,62 @@ +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' + compile pathfinder() +} + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) +} diff --git a/2019robot/gradle/wrapper/gradle-wrapper.jar b/2019robot/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..457aad0 Binary files /dev/null and b/2019robot/gradle/wrapper/gradle-wrapper.jar differ diff --git a/2019robot/gradle/wrapper/gradle-wrapper.properties b/2019robot/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..d08253c --- /dev/null +++ b/2019robot/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-5.0-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/2019robot/gradlew b/2019robot/gradlew new file mode 100644 index 0000000..af6708f --- /dev/null +++ b/2019robot/gradlew @@ -0,0 +1,172 @@ +#!/usr/bin/env sh + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >/dev/null +APP_HOME="`pwd -P`" +cd "$SAVED" >/dev/null + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn () { + echo "$*" +} + +die () { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; + NONSTOP* ) + nonstop=true + ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin, switch paths to Windows format before running java +if $cygwin ; then + APP_HOME=`cygpath --path --mixed "$APP_HOME"` + CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + JAVACMD=`cygpath --unix "$JAVACMD"` + + # We build the pattern for arguments to be converted via cygpath + ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` + SEP="" + for dir in $ROOTDIRSRAW ; do + ROOTDIRS="$ROOTDIRS$SEP$dir" + SEP="|" + done + OURCYGPATTERN="(^($ROOTDIRS))" + # Add a user-defined pattern to the cygpath arguments + if [ "$GRADLE_CYGPATTERN" != "" ] ; then + OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" + fi + # Now convert the arguments - kludge to limit ourselves to /bin/sh + i=0 + for arg in "$@" ; do + CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` + CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option + + if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition + eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` + else + eval `echo args$i`="\"$arg\"" + fi + i=$((i+1)) + done + case $i in + (0) set -- ;; + (1) set -- "$args0" ;; + (2) set -- "$args0" "$args1" ;; + (3) set -- "$args0" "$args1" "$args2" ;; + (4) set -- "$args0" "$args1" "$args2" "$args3" ;; + (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; + (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; + (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; + (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; + (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; + esac +fi + +# Escape application args +save () { + for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done + echo " " +} +APP_ARGS=$(save "$@") + +# Collect all arguments for the java command, following the shell quoting and substitution rules +eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" + +# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong +if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then + cd "$(dirname "$0")" +fi + +exec "$JAVACMD" "$@" diff --git a/2019robot/gradlew.bat b/2019robot/gradlew.bat new file mode 100644 index 0000000..6d57edc --- /dev/null +++ b/2019robot/gradlew.bat @@ -0,0 +1,84 @@ +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windows variants + +if not "%OS%" == "Windows_NT" goto win9xME_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/2019robot/settings.gradle b/2019robot/settings.gradle new file mode 100644 index 0000000..b0f4d48 --- /dev/null +++ b/2019robot/settings.gradle @@ -0,0 +1,25 @@ +import org.gradle.internal.os.OperatingSystem + +pluginManagement { + repositories { + mavenLocal() + gradlePluginPortal() + String frcYear = '2019' + File frcHome + if (OperatingSystem.current().isWindows()) { + String publicFolder = System.getenv('PUBLIC') + if (publicFolder == null) { + publicFolder = "C:\\Users\\Public" + } + frcHome = new File(publicFolder, "frc${frcYear}") + } else { + def userFolder = System.getProperty("user.home") + frcHome = new File(userFolder, "frc${frcYear}") + } + def frcHomeMaven = new File(frcHome, 'maven') + maven { + name 'frcHome' + url frcHomeMaven + } + } +} diff --git a/2019robot/src/main/deploy/example.txt b/2019robot/src/main/deploy/example.txt new file mode 100644 index 0000000..70c79b6 --- /dev/null +++ b/2019robot/src/main/deploy/example.txt @@ -0,0 +1,3 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'FileUtilities.getFilePath' wpilib function +to get a proper path relative to the deploy directory. \ No newline at end of file diff --git a/2019robot/src/main/java/buttons/XBoxTriggerButton.java b/2019robot/src/main/java/buttons/XBoxTriggerButton.java new file mode 100644 index 0000000..7abfe7c --- /dev/null +++ b/2019robot/src/main/java/buttons/XBoxTriggerButton.java @@ -0,0 +1,61 @@ +package buttons; + +import org.usfirst.frc4388.controller.XboxController; + +import edu.wpi.first.wpilibj.buttons.Button; + +public class XBoxTriggerButton extends Button +{ + public static final int RIGHT_TRIGGER = 0; + public static final int LEFT_TRIGGER = 1; + public static final int RIGHT_AXIS_UP_TRIGGER = 2; + public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; + public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; + public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; + public static final int LEFT_AXIS_UP_TRIGGER = 6; + public static final int LEFT_AXIS_DOWN_TRIGGER = 7; + public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; + public static final int LEFT_AXIS_LEFT_TRIGGER = 9; + + private XboxController m_controller; + private int m_trigger; + + public XBoxTriggerButton(XboxController controller, int trigger) { + m_controller = controller; + m_trigger = trigger; + } + + public boolean get() { + if (m_trigger == RIGHT_TRIGGER) { + return m_controller.getRightTrigger(); + } + else if (m_trigger == LEFT_TRIGGER) { + return m_controller.getLeftTrigger(); + } + else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { + return m_controller.getRightAxisUpTrigger(); + } + else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { + return m_controller.getRightAxisDownTrigger(); + } + else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { + return m_controller.getRightAxisRightTrigger(); + } + else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { + return m_controller.getRightAxisLeftTrigger(); + } + else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { + return m_controller.getLeftAxisUpTrigger(); + } + else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { + return m_controller.getLeftAxisDownTrigger(); + } + else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { + return m_controller.getLeftAxisRightTrigger(); + } + else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { + return m_controller.getLeftAxisLeftTrigger(); + } + return false; + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java b/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java new file mode 100644 index 0000000..d8f08dc --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/controller/IHandController.java @@ -0,0 +1,12 @@ +package org.usfirst.frc4388.controller; + +public interface IHandController { + + public double getLeftXAxis(); + + public double getLeftYAxis(); + + public double getRightXAxis(); + + public double getRightYAxis(); +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java b/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java new file mode 100644 index 0000000..040282a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/controller/XboxController.java @@ -0,0 +1,205 @@ + +package org.usfirst.frc4388.controller; + +import edu.wpi.first.wpilibj.Joystick; + +/** + * This is a wrapper for the WPILib Joystick class that represents an XBox + * controller. + * @author frc1675 + */ +public class XboxController implements IHandController +{ + public static final int LEFT_X_AXIS = 0; + public static final int LEFT_Y_AXIS = 1; + public static final int LEFT_TRIGGER_AXIS = 2; + public static final int RIGHT_TRIGGER_AXIS = 3; + public static final int RIGHT_X_AXIS = 4; + public static final int RIGHT_Y_AXIS = 5; + public static final int LEFT_RIGHT_DPAD_AXIS = 6; + public static final int TOP_BOTTOM_DPAD_AXIS = 6; + + public static final int A_BUTTON = 1; + public static final int B_BUTTON = 2; + public static final int X_BUTTON = 3; + public static final int Y_BUTTON = 4; + public static final int LEFT_BUMPER_BUTTON = 5; + public static final int RIGHT_BUMPER_BUTTON = 6; + public static final int BACK_BUTTON = 7; + public static final int START_BUTTON = 8; + + public static final int LEFT_JOYSTICK_BUTTON = 9; + public static final int RIGHT_JOYSTICK_BUTTON = 10; + + private static final double LEFT_DPAD_TOLERANCE = -0.9; + private static final double RIGHT_DPAD_TOLERANCE = 0.9; + private static final double BOTTOM_DPAD_TOLERANCE = -0.9; + private static final double TOP_DPAD_TOLERANCE = 0.9; + + private static final double LEFT_TRIGGER_TOLERANCE = 0.5; + private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; + + private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; + private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; + private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; + private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; + + private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; + private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; + private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; + private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; + + private static final double DEADZONE = 0.1; + + private Joystick stick; + + public XboxController(int portNumber){ + stick = new Joystick(portNumber); + } + + public Joystick getJoyStick() { + return stick; + } + + private boolean inDeadZone(double input){ + boolean inDeadZone; + if(Math.abs(input) < DEADZONE){ + inDeadZone = true; + }else{ + inDeadZone = false; + } + return inDeadZone; + } + + private double getAxisWithDeadZoneCheck(double input){ + if(inDeadZone(input)){ + input = 0.0; + } + return input; + } + + public boolean getAButton(){ + return stick.getRawButton(A_BUTTON); + } + + public boolean getXButton(){ + return stick.getRawButton(X_BUTTON); + } + + public boolean getBButton(){ + return stick.getRawButton(B_BUTTON); + } + + public boolean getYButton(){ + return stick.getRawButton(Y_BUTTON); + } + + public boolean getBackButton(){ + return stick.getRawButton(BACK_BUTTON); + } + + public boolean getStartButton(){ + return stick.getRawButton(START_BUTTON); + } + + public boolean getLeftBumperButton(){ + return stick.getRawButton(LEFT_BUMPER_BUTTON); + } + + public boolean getRightBumperButton(){ + return stick.getRawButton(RIGHT_BUMPER_BUTTON); + } + + public boolean getLeftJoystickButton(){ + return stick.getRawButton(LEFT_JOYSTICK_BUTTON); + } + + public boolean getRightJoystickButton(){ + return stick.getRawButton(RIGHT_JOYSTICK_BUTTON); + } + + public double getLeftXAxis(){ + return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_X_AXIS)); + } + + public double getLeftYAxis(){ + return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_Y_AXIS)); + } + + public double getRightXAxis(){ + return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_X_AXIS)); + } + + public double getRightYAxis(){ + return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_Y_AXIS)); + } + + public double getLeftTriggerAxis(){ + return getAxisWithDeadZoneCheck(stick.getRawAxis(LEFT_TRIGGER_AXIS)); + } + + public double getRightTriggerAxis(){ + return getAxisWithDeadZoneCheck(stick.getRawAxis(RIGHT_TRIGGER_AXIS)); + } + + /**Returns -1 if nothing is pressed, or the angle of the button pressed 0 = up, 90 = right, etc.*/ + public int getDpadAngle() { + return stick.getPOV(); + } + + public boolean getDPadLeft(){ + return (stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); + } + + public boolean getDPadRight(){ + return (stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); + } + + public boolean getDPadTop(){ + return (stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); + } + + /*public boolean getDPadBottom(){ + return (stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); + } +*/ + public boolean getLeftTrigger(){ + return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); + } + + public boolean getRightTrigger(){ + return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); + } + + public boolean getRightAxisUpTrigger(){ + return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); + } + + public boolean getRightAxisDownTrigger(){ + return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); + } + + public boolean getRightAxisLeftTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); + } + + public boolean getRightAxisRightTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); + } + + public boolean getLeftAxisUpTrigger(){ + return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); + } + + public boolean getLeftAxisDownTrigger(){ + return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); + } + + public boolean getLeftAxisLeftTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); + } + + public boolean getLeftAxisRightTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java new file mode 100644 index 0000000..96a6f89 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Constants.java @@ -0,0 +1,72 @@ + +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 kElevatorEncoderTickesPerRev = 256; + public static double kElevatorInchesOfTravelPerRev = 3.75; + public static double kElevatorEncoderTicksPerInch = 126.36; + public static double kElevatorBasicPercentOutputUp = -0.85; + public static double kElevatorBasicPercentOutputDown =.7; + + // CONTROL LOOP GAINS + + // PID gains for drive velocity loop (LOW GEAR) + // Units: error is 4096 counts/rev. Max output is +/- 1023 units. + public static double kDriveVelocityKp = 1.0; + public static double kDriveVelocityKi = 0.0; + public static double kDriveVelocityKd = 6.0; + public static double kDriveVelocityKf = 0.5; + public static int kDriveVelocityIZone = 0; + public static double kDriveVelocityRampRate = 0.0; + public static int kDriveVelocityAllowableError = 0; + + // PID gains for drive base lock loop + // Units: error is 4096 counts/rev. Max output is +/- 1023 units. + public static double kDriveBaseLockKp = 0.5; + public static double kDriveBaseLockKi = 0; + public static double kDriveBaseLockKd = 0; + public static double kDriveBaseLockKf = 0; + public static int kDriveBaseLockIZone = 0; + public static double kDriveBaseLockRampRate = 0; + public static int kDriveBaseLockAllowableError = 10; + + // PID gains for constant heading velocity control + // Units: Error is degrees. Output is inches/second difference to + // left/right. + public static double kDriveHeadingVelocityKp = 4.0; // 6.0; + public static double kDriveHeadingVelocityKi = 0.0; + public static double kDriveHeadingVelocityKd = 50.0; + + // Path following constants + public static double kPathFollowingLookahead = 24.0; // inches + public static double kPathFollowingMaxVel = 120.0; // inches/sec + public static double kPathFollowingMaxAccel = 80.0; // inches/sec^2 + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java new file mode 100644 index 0000000..91b4cae --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Main.java @@ -0,0 +1,22 @@ +package org.usfirst.frc4388.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java new file mode 100644 index 0000000..4fc1133 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -0,0 +1,101 @@ + + + +package org.usfirst.frc4388.robot; + +import buttons.XBoxTriggerButton; +import org.usfirst.frc4388.controller.IHandController; +import org.usfirst.frc4388.controller.XboxController; +import org.usfirst.frc4388.robot.commands.*; + + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.*; +import org.usfirst.frc4388.robot.subsystems.*; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; +import org.usfirst.frc4388.robot.subsystems.Drive; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj.buttons.InternalButton; +import edu.wpi.first.wpilibj.buttons.JoystickButton; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import jaci.pathfinder.Pathfinder; + + + +public class OI +{ + private static OI instance; + + private XboxController m_driverXbox; + private XboxController m_operatorXbox; + + private OI() + { + try + { + // Driver controller + m_driverXbox = new XboxController(RobotMap.DRIVER_JOYSTICK_1_USB_ID); + m_operatorXbox = new XboxController(RobotMap.OPERATOR_JOYSTICK_1_USB_ID); + + /* XBoxTriggerButton CarriageIntake = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.LEFT_TRIGGER); + CarriageIntake.whenPressed(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + CarriageIntake.whenReleased(new IntakeSetSpeed(0.0)); + + XBoxTriggerButton CarriageEject = new XBoxTriggerButton(m_operatorXbox, XBoxTriggerButton.RIGHT_TRIGGER); + CarriageEject.whenPressed(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); + CarriageEject.whenReleased(new IntakeSetSpeed(0.0)); + */ + JoystickButton climbUp = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); + climbUp.whenPressed(new InitiateClimber(true)); + climbUp.whenReleased(new InitiateClimber(false)); + + JoystickButton shiftUp = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); + shiftUp.whenPressed(new DriveSpeedShift(true)); + // shiftUp.whenPressed(new LEDIndicators(true)); + + JoystickButton shiftDown = new JoystickButton(m_driverXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); + shiftDown.whenPressed(new DriveSpeedShift(false)); + // shiftDown.whenPressed(new LEDIndicators(false)); + + + //Operator Xbox +/* + JoystickButton openIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.LEFT_BUMPER_BUTTON); + openIntake.whenPressed(new IntakePosition(true)); + + JoystickButton CloseIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); + CloseIntake.whenPressed(new IntakePosition(false)); + + SmartDashboard.putData("PRE GAME!!!!", new PreGame()); + */ + } catch (Exception e) { + System.err.println("An error occurred in the OI constructor"); + } + } + + public static OI getInstance() { + if(instance == null) { + instance = new OI(); + } + return instance; + } + + public IHandController getDriverController() { + return m_driverXbox; + } + + public IHandController getOperatorController() + { + return m_operatorXbox; + } + + public Joystick getOperatorJoystick() + { + return m_operatorXbox.getJoyStick(); + } + } + diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java new file mode 100644 index 0000000..36fea3e --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -0,0 +1,163 @@ + +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 Elevator elevator = new Elevator(); + + + + public static final Climber climber = new Climber(); + public static final Pnumatics pnumatics = new Pnumatics(); + public static final long periodMS = 10; + public static final ControlLooper controlLoop = new ControlLooper("Main control loop", periodMS); + + public static enum OperationMode { TEST, COMPETITION }; + public static OperationMode operationMode = OperationMode.TEST; + + private SendableChooser operationModeChooser; + private SendableChooser RRautonTaskChooser; + private SendableChooser RLautonTaskChooser; + private SendableChooser LRautonTaskChooser; + private SendableChooser LLautonTaskChooser; + + private Command RRautonomousCommand; + private Command RLautonomousCommand; + private Command LRautonomousCommand; + private Command LLautonomousCommand; + + public void robotInit() + { + //Printing game data to riolog + String gameData = DriverStation.getInstance().getGameSpecificMessage(); + System.out.println(gameData); + CameraServer.getInstance().startAutomaticCapture(); + CameraServer.getInstance().putVideo("res", 300, 220); + + try { + oi = OI.getInstance(); + + controlLoop.addLoopable(drive); + controlLoop.addLoopable(elevator); + + + operationModeChooser = new SendableChooser(); + operationModeChooser.addDefault("Test", OperationMode.TEST); + operationModeChooser.addObject("Competition", OperationMode.COMPETITION); + SmartDashboard.putData("Operation Mode", operationModeChooser); + + + + + + + + + //ledLights.setAllLightsOn(false); + } catch (Exception e) { + System.err.println("An error occurred in robotInit()"); + } + } + + public void disabledInit(){ + + } + + public void disabledPeriodic() { + Scheduler.getInstance().run(); + updateStatus(); + } + + public void autonomousInit() { + updateChoosers(); + + controlLoop.start(); + drive.endGyroCalibration(); + drive.resetEncoders(); + drive.resetGyro(); + drive.setIsRed(getAlliance().equals(Alliance.Red)); + + } + + + + public void autonomousPeriodic() { + Scheduler.getInstance().run(); + updateStatus(); + } + + public void teleopInit() { + if (RRautonomousCommand != null) RRautonomousCommand.cancel(); + if (RLautonomousCommand != null) RLautonomousCommand.cancel(); + if (LRautonomousCommand != null) LRautonomousCommand.cancel(); + if (LLautonomousCommand != null) LLautonomousCommand.cancel(); + drive.setToBrakeOnNeutral(false); + updateChoosers(); + controlLoop.start(); + drive.resetEncoders(); + drive.endGyroCalibration(); + + updateStatus(); + } + + + public void teleopPeriodic() + { + Scheduler.getInstance().run(); + updateStatus(); + } + + public void testPeriodic() { + LiveWindow.run(); + updateStatus(); + } + + private void updateChoosers() { + operationMode = (OperationMode)operationModeChooser.getSelected(); + RRautonomousCommand = (Command)RRautonTaskChooser.getSelected(); + RLautonomousCommand = (Command)RLautonTaskChooser.getSelected(); + LRautonomousCommand = (Command)LRautonTaskChooser.getSelected(); + LLautonomousCommand = (Command)LLautonTaskChooser.getSelected(); + } + + public Alliance getAlliance() { + return m_ds.getAlliance(); + } + + public void updateStatus() { + drive.updateStatus(operationMode); + elevator.updateStatus(operationMode); + + } + +} + diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java new file mode 100644 index 0000000..64e015f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -0,0 +1,36 @@ + +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; + + //Elevator Motors + public static final int ELEVATOR_MOTOR1_ID = 6; + public static final int ELEVATOR_MOTOR2_ID = 7; + public static final int CLIMBER_CAN_ID = 10; + + + // Pneumatics + public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0; + public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; + public static final int OPEN_INTAKE_PCM_ID = 4; + public static final int CLOSE_INTAKE_PCM_ID = 5; + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java new file mode 100644 index 0000000..5c25540 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveAbsoluteTurnMP.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.Command; + +public class DriveAbsoluteTurnMP extends Command +{ + private double absoluteTurnAngleDeg, maxTurnRateDegPerSec; + private MPSoftwareTurnType turnType; + + public DriveAbsoluteTurnMP(double absoluteTurnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) { + requires(Robot.drive); + this.absoluteTurnAngleDeg = absoluteTurnAngleDeg; + this.maxTurnRateDegPerSec = maxTurnRateDegPerSec; + this.turnType = turnType; + } + + protected void initialize() { +// if (Robot.drive.isRed() == false) { +// absoluteTurnAngleDeg = absoluteTurnAngleDeg * -1; +// } + Robot.drive.setAbsoluteTurnMP(absoluteTurnAngleDeg, maxTurnRateDegPerSec, turnType); + } + + protected void execute() { + } + + protected boolean isFinished() { + return Robot.drive.isFinished(); + } + + protected void end() { + Robot.drive.setControlMode(DriveControlMode.JOYSTICK); + } + + protected void interrupted() { + end(); + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java new file mode 100644 index 0000000..9d2d3e4 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveGyroReset.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class DriveGyroReset extends Command +{ + public DriveGyroReset() { + requires(Robot.drive); + } + + @Override + protected void initialize() { + Robot.drive.resetGyro(); + Robot.drive.resetEncoders(); + } + + @Override + protected void execute() { + + } + + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end() { + + } + + @Override + protected void interrupted() { + + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java new file mode 100644 index 0000000..0bc270b --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveRelativeTurnPID.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.Command; + +public class DriveRelativeTurnPID extends Command +{ + private double relativeTurnAngleDeg; + private MPSoftwareTurnType turnType; + + public DriveRelativeTurnPID(double relativeTurnAngleDeg, MPSoftwareTurnType turnType) { + requires(Robot.drive); + this.relativeTurnAngleDeg = relativeTurnAngleDeg; + this.turnType = turnType; + } + + protected void initialize() { + Robot.drive.setRelativeTurnPID(relativeTurnAngleDeg, 0.3, 0.1, turnType); + } + + protected void execute() { + } + + protected boolean isFinished() { + return Robot.drive.isFinished(); + } + + protected void end() { + Robot.drive.setControlMode(DriveControlMode.JOYSTICK); + } + + protected void interrupted() { + end(); + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java new file mode 100644 index 0000000..d3e8293 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveSpeedShift.java @@ -0,0 +1,40 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class DriveSpeedShift extends Command +{ + public boolean speed; + + public DriveSpeedShift(boolean speed) { + this.speed=speed; + requires(Robot.pnumatics); + } + + @Override + protected void initialize() { + Robot.pnumatics.setShiftState(speed); + } + + @Override + protected void execute() { + + } + + @Override + protected boolean isFinished() { + return true; + } + + @Override + protected void end() { + + } + + @Override + protected void interrupted() { + + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java new file mode 100644 index 0000000..a477dc7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java @@ -0,0 +1,103 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +public class DriveStraightBasic extends Command { + private double m_targetInches; + private double m_maxVelocityInchesPerSec; + private double m_speed; + private boolean m_goingBackwards; + private boolean m_useGyroLock; + private boolean m_useAbsolute; + private double m_desiredAbsoluteAngle; + private double m_commandInitTimestamp; + private double m_lastCommandExecuteTimestamp = 0.0; + private double m_lastCommandExecutePeriod = 0.0; + + public DriveStraightBasic(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + requires(Robot.drive); + m_targetInches = targetInches; + m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; + m_useGyroLock = useGyroLock; + m_useAbsolute = useAbsolute; + m_desiredAbsoluteAngle = desiredAbsoluteAngle; + m_goingBackwards = (m_targetInches < 0.0); + } + + protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { + double sign = (backwards ? -1.0 : 1.0); + // Keep velocity above stiction limit (else bot will freeze and command will not complete) + double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); + // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) + return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.drive.resetGyro(); + Robot.drive.resetEncoders(); + Robot.drive.setControlMode(DriveControlMode.RAW); + // start out at half speed, as crude way to reduce slippage + m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); + m_commandInitTimestamp = Timer.getFPGATimestamp(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + // Measure *actual* update period + double currentTimestamp = Timer.getFPGATimestamp(); + if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time + m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; + } + m_lastCommandExecuteTimestamp = currentTimestamp; + double steer = 0.0; + if (m_useGyroLock) { + steer = - Robot.drive.getGyroAngleDeg() / 25.0; //TODO: tune + } + Robot.drive.rawMoveSteer(m_speed, steer); + //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + boolean finished=false; + double velocity = m_maxVelocityInchesPerSec; + double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2; + double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); + if (remaining < 0.0) { + finished = true; + } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms + velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed + } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms + velocity = m_maxVelocityInchesPerSec / 2.0; // half speed + } + if (!finished) { + m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); + SmartDashboard.putNumber("DSB Dist", position); + } else { + SmartDashboard.putNumber("DSB finDist", position); + } + return finished; + } + + // Called once after isFinished returns true + protected void end() { + double currentTimestamp = Timer.getFPGATimestamp(); + SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); + Robot.drive.rawMoveSteer(0.0, 0.0); + Robot.drive.setControlMode(DriveControlMode.JOYSTICK); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java new file mode 100644 index 0000000..21fede7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java @@ -0,0 +1,115 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +public class DriveStraightBasicAbs extends Command { + private double m_targetInches; + private double m_maxVelocityInchesPerSec; + private double m_speed; + private boolean m_goingBackwards; + private boolean m_useGyroLock; + private boolean m_useAbsolute; + private double m_desiredAbsoluteAngle; + private double m_targetGyroAngleDeg; // what we want the gyro to read as we're driving straight + private double m_commandInitTimestamp; + private double m_lastCommandExecuteTimestamp = 0.0; + private double m_lastCommandExecutePeriod = 0.0; + + public DriveStraightBasicAbs(double targetInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + requires(Robot.drive); + m_targetInches = targetInches; + m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; + m_useGyroLock = useGyroLock; + m_useAbsolute = useAbsolute; + m_desiredAbsoluteAngle = desiredAbsoluteAngle; + m_goingBackwards = (m_targetInches < 0.0); + } + + protected double velocityToMoveSpeed(double velocityInchesPerSec, boolean backwards) { + double sign = (backwards ? -1.0 : 1.0); + // Keep velocity above stiction limit (else bot will freeze and command will not complete) + double velocity = Math.max(Constants.kDriveStraightBasicMinSpeedInchesPerSec, velocityInchesPerSec); + // Figure out move value based on percentage of measured max speed (i.e. that at full 1.0 joystick) + return sign * velocity / Constants.kDriveStraightBasicMaxSpeedInchesPerSec; + } + + // Called just before this Command runs the first time + protected void initialize() { + double currentAngleDeg = Robot.drive.getGyroAngleDeg(); + if (m_useAbsolute) { + m_targetGyroAngleDeg = m_desiredAbsoluteAngle; + } else { + m_targetGyroAngleDeg = currentAngleDeg; + } + Robot.drive.resetEncoders(); + Robot.drive.setControlMode(DriveControlMode.RAW); + // start out at half speed, as crude way to reduce slippage + m_speed = velocityToMoveSpeed(m_maxVelocityInchesPerSec/2.0, m_goingBackwards); + m_commandInitTimestamp = Timer.getFPGATimestamp(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + // Measure *actual* update period + double currentTimestamp = Timer.getFPGATimestamp(); + if (m_lastCommandExecuteTimestamp > 0.001) { // ie, if this is NOT the first time + m_lastCommandExecutePeriod = currentTimestamp - m_lastCommandExecuteTimestamp; + } + m_lastCommandExecuteTimestamp = currentTimestamp; + double steer = 0.0; + if (m_useGyroLock) { + double yawError = Robot.drive.getGyroAngleDeg() - m_targetGyroAngleDeg; + steer = -yawError / Constants.kDriveStraightBasicYawErrorDivisor; + if (steer < -Constants.kDriveStraightBasicMaxSteerMagnitude) { + steer = -Constants.kDriveStraightBasicMaxSteerMagnitude; + } else if (Constants.kDriveStraightBasicMaxSteerMagnitude < steer) { + steer = Constants.kDriveStraightBasicMaxSteerMagnitude; + } + } + Robot.drive.rawMoveSteer(m_speed, steer); + //SmartDashboard.putNumber("DSB Period", m_lastCommandExecutePeriod); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + boolean finished=false; + double velocity = m_maxVelocityInchesPerSec; + double position = (Robot.drive.getLeftPositionWorld() + Robot.drive.getRightPositionWorld())/2; + double remaining = (m_targetInches - position) * (m_goingBackwards ? -1.0 : 1.0); + if (remaining < 0.0) { + finished = true; + } else if (remaining < 0.1 * m_maxVelocityInchesPerSec / 2.0) { // last 100 ms + velocity = m_maxVelocityInchesPerSec / 4.0; // quarter speed + } else if (remaining < 0.3 * m_maxVelocityInchesPerSec) { // last 300 ms + velocity = m_maxVelocityInchesPerSec / 2.0; // half speed + } + if (!finished) { + m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); + SmartDashboard.putNumber("DSB Dist", position); + } else { + SmartDashboard.putNumber("DSB finDist", position); + } + return finished; + } + + // Called once after isFinished returns true + protected void end() { + double currentTimestamp = Timer.getFPGATimestamp(); + SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); + Robot.drive.rawMoveSteer(0.0, 0.0); + Robot.drive.setControlMode(DriveControlMode.JOYSTICK); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java new file mode 100644 index 0000000..e205818 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightMP.java @@ -0,0 +1,60 @@ +// RobotBuilder Version: 2.0 +// +// This file was generated by RobotBuilder. It contains sections of +// code that are automatically generated and assigned by robotbuilder. +// These sections will be updated in the future when you export to +// Java from RobotBuilder. Do not put any code or make any change in +// the blocks indicating autogenerated code or it will be lost on an +// update. Deleting the comments indicating the section will prevent +// it from being updated in the future. + + +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + + +public class DriveStraightMP extends Command { + private double m_distanceInches; + private double m_maxVelocityInchesPerSec; + private boolean m_useGyroLock; + private boolean m_useAbsolute; + private double m_desiredAbsoluteAngle; + + public DriveStraightMP(double distanceInches, double maxVelocityInchesPerSec, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + requires(Robot.drive); + m_distanceInches = distanceInches; + m_maxVelocityInchesPerSec = maxVelocityInchesPerSec; + m_useGyroLock = useGyroLock; + m_useAbsolute = useAbsolute; + m_desiredAbsoluteAngle = desiredAbsoluteAngle; + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.drive.setStraightMP(m_distanceInches, m_maxVelocityInchesPerSec, m_useGyroLock, m_useAbsolute, m_desiredAbsoluteAngle); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return Robot.drive.isFinished(); + } + + // Called once after isFinished returns true + protected void end() { + Robot.drive.setControlMode(DriveControlMode.JOYSTICK); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java new file mode 100644 index 0000000..82223d9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveTurnBasic.java @@ -0,0 +1,151 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.utility.BHRMathUtils; +import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.command.Command; + +public class DriveTurnBasic extends Command +{ + private boolean m_useAbsolute; // if true, we don't reset the gyro before starting the turn + private double m_turnAngleDeg; // either absolute or relative to current heading, depending on useAbsolute + private double m_maxTurnRateDegPerSec; + private MPSoftwareTurnType m_turnType; + private boolean m_turningLeft; + private double m_targetGyroAngleDeg; // what we want the gyro to read when we're done + + public DriveTurnBasic(boolean useAbsolute, double turnAngleDeg, double maxTurnRateDegPerSec, MPSoftwareTurnType turnType) { + requires(Robot.drive); + m_useAbsolute = useAbsolute; + m_turnAngleDeg = turnAngleDeg; + m_maxTurnRateDegPerSec = maxTurnRateDegPerSec; + m_turnType = turnType; + } + + protected void initialize() { + double currentAngleDeg = Robot.drive.getGyroAngleDeg(); + if (m_useAbsolute) { + m_targetGyroAngleDeg = m_turnAngleDeg; + } else { + m_targetGyroAngleDeg = currentAngleDeg + m_turnAngleDeg; + } + if ((m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.RIGHT_ARC)) { + m_turningLeft = true; // gyro angle will be DEcreasing, so make sure we aim for an angle in that direction + while (m_targetGyroAngleDeg >= currentAngleDeg) { + m_targetGyroAngleDeg = m_targetGyroAngleDeg - 360.0; + } + } else if ((m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) || (m_turnType == MPSoftwareTurnType.LEFT_ARC)) { + m_turningLeft = false; // gyro angle will be INcreasing, so make sure we aim for an angle in that direction + while (m_targetGyroAngleDeg <= currentAngleDeg) { + m_targetGyroAngleDeg = m_targetGyroAngleDeg + 360.0; + } + } else { // MPSoftwareTurnType.TANK -- need to determine based on values passed + if (m_useAbsolute) { // this is the only case where we have to DECIDE which direction to turn + m_turnAngleDeg = BHRMathUtils.normalizeAngle0To360(m_turnAngleDeg); + m_targetGyroAngleDeg = BHRMathUtils.adjustAccumAngleToDesired(currentAngleDeg, m_turnAngleDeg); + m_turningLeft = (m_targetGyroAngleDeg < currentAngleDeg); + } else { + m_turningLeft = (m_turnAngleDeg < 0); + } + } + System.out.println("Turning " + (m_turningLeft?"left":"right") + " from " + currentAngleDeg + " to " + m_targetGyroAngleDeg + " degrees"); + Robot.drive.setControlMode(DriveControlMode.RAW); + Robot.drive.resetEncoders(); + } + + protected void execute() { + double output = Constants.kDriveTurnBasicSingleMotorOutput; + + if (m_turnType == MPSoftwareTurnType.TANK) { + output = Constants.kDriveTurnBasicTankMotorOutput; + if (m_turningLeft) { + Robot.drive.rawDriveLeftRight(-output, -output); // left backward, right forward + } else { + Robot.drive.rawDriveLeftRight(output, output); // left forward, right backward + } +// for (CANTalonEncoder motorController : motorControllers) { +// //motorController.set(output); +// motorController.set(ControlMode.PercentOutput, output); +// } + } + else if (m_turnType == MPSoftwareTurnType.LEFT_SIDE_ONLY) { + Robot.drive.rawDriveLeftRight(2.0 * output, 0.0); // left forward double speed +// for (CANTalonEncoder motorController : motorControllers) { +// if (motorController.isRight()) { +// //motorController.set(0); +// motorController.set(ControlMode.PercentOutput, 0); +// } +// else { +// //motorController.set(2.0 * output); +// motorController.set(ControlMode.PercentOutput, 2.0 * output); +// } +// } + } + else if (m_turnType == MPSoftwareTurnType.RIGHT_SIDE_ONLY) { + Robot.drive.rawDriveLeftRight(0.0, -2.0 * output); // right forward double speed +// for (CANTalonEncoder motorController : motorControllers) { +// if (motorController.isRight()) { +// //motorController.set(2.0 * output); +// motorController.set(ControlMode.PercentOutput, 2.0 * output); +// } +// else { +// //motorController.set(0); +// motorController.set(ControlMode.PercentOutput, 0); +// } +// } + } + else if (m_turnType == MPSoftwareTurnType.LEFT_ARC) { + Robot.drive.rawDriveLeftRight(2.0 * output, -output); // left fwd 2x, right fwd 1x +// for (CANTalonEncoder motorController : motorControllers) { +// if (motorController.isRight()) { +// //motorController.set(1.0 * output); +// motorController.set(ControlMode.PercentOutput, 1.0 * output); +// } +// else { +// //motorController.set(2.0 * output); +// motorController.set(ControlMode.PercentOutput, 2.0 * output); +// } +// } + } + else if (m_turnType == MPSoftwareTurnType.RIGHT_ARC) { + Robot.drive.rawDriveLeftRight(output, -2.0 * output); // left fwd 1x, right fwd 2x +// for (CANTalonEncoder motorController : motorControllers) { +// if (motorController.isRight()) { +// //motorController.set(2.0 * output); +// motorController.set(ControlMode.PercentOutput, 2.0 * output); +// } +// else { +// //motorController.set(1.0 * output); +// motorController.set(ControlMode.PercentOutput, 1.0 * output); +// } +// } + } + } + + protected boolean isFinished() { + boolean finished; + double currentAngleDeg = Robot.drive.getGyroAngleDeg(); + + if (m_turningLeft) { + finished = currentAngleDeg <= m_targetGyroAngleDeg; + } else { + finished = currentAngleDeg >= m_targetGyroAngleDeg; + } + return finished; + } + + protected void end() { + Robot.drive.rawDriveLeftRight(0.0, 0.0); + Robot.drive.setControlMode(DriveControlMode.JOYSTICK); + } + + protected void interrupted() { + end(); + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java new file mode 100644 index 0000000..df2b718 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorBasic.java @@ -0,0 +1,99 @@ +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 ElevatorBasic 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 ElevatorBasic(double targetHeightInchesAboveFloor) { + requires(Robot.elevator); + m_targetHeightInchesAboveFloor = targetHeightInchesAboveFloor; + } + + // Called just before this Command runs the first time + protected void initialize() + { + Robot.elevator.setControlMode(DriveControlMode.RAW); + + double currentHeight = Robot.elevator.getElevatorHeightInchesAboveFloor(); + // 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.kElevatorBasicPercentOutputUp; + } + else { + m_speed = Constants.kElevatorBasicPercentOutputDown; + } + 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.elevator.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.elevator.getElevatorHeightInchesAboveFloor(); + 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.elevator.rawSetOutput(0.0); + + Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java new file mode 100644 index 0000000..7553d48 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ElevatorSetSpeed.java @@ -0,0 +1,44 @@ +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 ElevatorSetSpeed extends Command { + + private double RaiseSpeed; + + // Constructor with speed + public ElevatorSetSpeed(double RaiseSpeed) { + this.RaiseSpeed = RaiseSpeed; + // requires(Robot.elevatorAuton); + } + + // Called just before this Command runs the first time + protected void initialize() { + //Robot.elevatorAuton.setRaiseSpeed(RaiseSpeed); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java new file mode 100644 index 0000000..8a6e37d --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/InitiateClimber.java @@ -0,0 +1,38 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +public class InitiateClimber extends Command +{ + boolean climb; + + public InitiateClimber(boolean climb) { + this.climb=climb; + requires(Robot.climber); + } + + @Override + protected void initialize() { + Robot.climber.setClimbSpeed(climb); + } + + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + protected void end() { + } + + @Override + protected void interrupted() { + + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..3cacf76 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Climber.java @@ -0,0 +1,68 @@ + + + + +package org.usfirst.frc4388.robot.subsystems; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.RobotMap; +import org.usfirst.frc4388.robot.commands.*; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + + +/** + * + */ +public class Climber extends Subsystem{ + + private WPI_TalonSRX Climber; + + public boolean on; + + public Climber(){ + + try{ + + Climber = new WPI_TalonSRX(RobotMap.CLIMBER_CAN_ID); + + } catch (Exception e) { + + System.err.println("An error occurred in the climbing constructor"); + + } + } + + @Override + public void initDefaultCommand() { + + } + + + @Override + public void periodic() { + // Put code here to be run every loop + + } + + public void setClimbSpeed(boolean Climb) { + if (Climb==true) { + Climber.set(1.0); + } + if (Climb==false) { + Climber.set(0); + } +} + // Put methods for controlling this subsystem + // here. Call these from Commands. + { + // TODO Auto-generated method stub + + } + +} + diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java new file mode 100644 index 0000000..b984af3 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -0,0 +1,863 @@ + +package org.usfirst.frc4388.robot.subsystems; + +import java.util.ArrayList; +import java.util.Set; + +import org.usfirst.frc4388.robot.Constants; +import org.usfirst.frc4388.robot.OI; +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.RobotMap; +import org.usfirst.frc4388.utility.AdaptivePurePursuitController; +import org.usfirst.frc4388.utility.BHRMathUtils; +import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.ControlLoopable; +import org.usfirst.frc4388.utility.Kinematics; +import org.usfirst.frc4388.utility.MMTalonPIDController; +import org.usfirst.frc4388.utility.MPSoftwarePIDController; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; +import org.usfirst.frc4388.utility.MPTalonPIDController; +import org.usfirst.frc4388.utility.MPTalonPIDPathController; +import org.usfirst.frc4388.utility.MPTalonPIDPathVelocityController; +//import org.usfirst.frc4388.utility.SoftwarePIDPositionController; +import org.usfirst.frc4388.utility.MotionProfilePoint; +//import org.usfirst.frc4388.utility.MotionProfileBoxCar; +import org.usfirst.frc4388.utility.PIDParams; +import org.usfirst.frc4388.utility.Path; +import org.usfirst.frc4388.utility.PathGenerator; +import org.usfirst.frc4388.utility.RigidTransform2d; +import org.usfirst.frc4388.utility.Rotation2d; +import org.usfirst.frc4388.utility.SoftwarePIDController; +import org.usfirst.frc4388.utility.Translation2d; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +//import com.ctre.PigeonImu; +//import com.ctre.PigeonImu.CalibrationMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import com.kauailabs.navx.frc.AHRS; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +//import edu.wpi.first.wpilibj.DigitalInput; +//import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +//import edu.wpi.first.wpilibj.Solenoid; +//import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +/** + * + */ +public class Drive extends Subsystem implements ControlLoopable +{ + public static enum DriveControlMode { JOYSTICK, MP_STRAIGHT, MP_TURN, PID_TURN, HOLD, MANUAL, CLIMB, MP_PATH, MP_PATH_VELOCITY, MOTION_MAGIC, ADAPTIVE_PURSUIT, MOVE_POSITION_MAX_SCALE, MOVE_POSITION_LOW_SCALE, MOVE_POSITION_SWITCH, MOVE_POSITION_LOWEST, STOP_MOTORS, RAW}; + public static enum SpeedShiftState { HI, LO }; + public static enum ClimberState { DEPLOYED, RETRACTED }; + + public static final double TRACK_WIDTH_INCHES = Constants.kTrackWidthInches; + public static final double ENCODER_TICKS_TO_INCHES = Constants.kDriveEncoderTicksPerInch; + + public static final double CLIMB_SPEED = 0.45; + + public static final double VOLTAGE_RAMP_RATE = 150; // Volts per second + public static final double OPEN_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full + public static final double CLOSED_LOOP_RAMP_SECONDS = 12.0 / VOLTAGE_RAMP_RATE; // Seconds from neutral to full + + // Motion profile max velocities and accel times + public static final double MAX_TURN_RATE_DEG_PER_SEC = 320; + public static final double MP_AUTON_MAX_STRAIGHT_VELOCITY_INCHES_PER_SEC = 120; //72; + public static final double MP_AUTON_MAX_BOILER_STRAIGHT_VELOCITY_INCHES_PER_SEC = 200; + public static final double MP_AUTON_MAX_LO_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 320; + public static final double MP_AUTON_MAX_HIGH_GEAR_STRAIGHT_VELOCITY_INCHES_PER_SEC = 400; + public static final double MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC = 270; + public static final double MP_AUTON_MAX_BOILER_TURN_RATE_DEG_PER_SEC = 400; + public static final double MP_GEAR_DEPLOY_VELOCITY_INCHES_PER_SEC = 25; + public static final double MP_GEAR_DEPLOY_FASTER_VELOCITY_INCHES_PER_SEC = 80; + + + + + public static final double MP_STRAIGHT_T1 = 600; + public static final double MP_STRAIGHT_T2 = 300; + public static final double MP_TURN_T1 = 600; + public static final double MP_TURN_T2 = 300; + public static final double MP_MAX_TURN_T1 = 400; + public static final double MP_MAX_TURN_T2 = 200; + + // Motor controllers + private ArrayList motorControllers = new ArrayList(); + + private CANTalonEncoder leftDrive1; + private WPI_TalonSRX leftDrive2; +// private WPI_TalonSRX leftDrive3; + + private CANTalonEncoder rightDrive1; + private WPI_TalonSRX rightDrive2; +// private WPI_TalonSRX rightDrive3; + + private DifferentialDrive m_drive; + + //PID encoder and motor + private CANTalonEncoder elevatorRight; + private WPI_TalonSRX elevatorLeft; + + //private DigitalInput hopperSensorRed; + //private DigitalInput hopperSensorBlue; + + + + private double climbSpeed; + + private boolean isRed = true; + + private double periodMs; + private double lastControlLoopUpdatePeriod = 0.0; + private double lastControlLoopUpdateTimestamp = 0.0; + + // Pneumatics + //private Solenoid speedShift; + + // Input devices + public static final int DRIVER_INPUT_JOYSTICK_ARCADE = 0; + public static final int DRIVER_INPUT_JOYSTICK_TANK = 1; + public static final int DRIVER_INPUT_JOYSTICK_CHEESY = 2; + public static final int DRIVER_INPUT_XBOX_CHEESY = 3; + public static final int DRIVER_INPUT_XBOX_ARCADE_LEFT = 4; + public static final int DRIVER_INPUT_XBOX_ARCADE_RIGHT = 5; + public static final int DRIVER_INPUT_WHEEL = 6; + + public static final double STEER_NON_LINEARITY = 0.5; + public static final double MOVE_NON_LINEARITY = 1.0; + + public static final double STICK_DEADBAND = 0.02; + + private int m_moveNonLinear = 0; + private int m_steerNonLinear = -3; + + private double m_moveScale = 1.0; + private double m_steerScale = 1.0; + + private double m_moveInput = 0.0; + private double m_steerInput = 0.0; + + private double m_moveOutput = 0.0; + private double m_steerOutput = 0.0; + + private double m_moveTrim = 0.0; + private double m_steerTrim = 0.0; + + private boolean isFinished; + private DriveControlMode controlMode = DriveControlMode.JOYSTICK; + + private MPTalonPIDController mpStraightController; + private PIDParams mpStraightPIDParams = new PIDParams(.7, 0.0, 0.000000, .001, 0.001, .7); // 4 colsons +// private PIDParams mpStraightPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.03); // 4 omni + private PIDParams mpHoldPIDParams = new PIDParams(1, 0, 0, 0.0, 0.0, 0.0); + + private MMTalonPIDController mmStraightController; + private PIDParams mmStraightPIDParams = new PIDParams(0, 0, 0, 0.24); + + private MPSoftwarePIDController mpTurnController; // p i d a v g izone + private PIDParams mpTurnPIDParams = new PIDParams(0.07, 0.00002, 0.5, 0.00025, 0.008, 0.0, 100); // 4 colson wheels +// private PIDParams mpTurnPIDParams = new PIDParams(0.03, 0.00002, 0.4, 0.0004, 0.0030, 0.0, 100); // 4 omni + + private SoftwarePIDController pidTurnController; + //private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.001, 0.4, 0, 0, 0.0, 100); //i=0.0008 + private PIDParams pidTurnPIDParams = new PIDParams(0.04, 0.0, 0.0, 0, 0, 0.0, 100); //i=0.0008 + private double targetPIDAngle; + + private MPTalonPIDPathController mpPathController; + private PIDParams mpPathPIDParams = new PIDParams(0.1, 0, 0, 0.005, 0.03, 0.28, 100); // 4 omni g=.3 + + //PID target + private double targetPIDPosition; + + private MPTalonPIDPathVelocityController mpPathVelocityController; + private PIDParams mpPathVelocityPIDParams = new PIDParams(0.5, 0.001, 5, 0.44); + + private AdaptivePurePursuitController adaptivePursuitController; + private PIDParams adaptivePursuitPIDParams = new PIDParams(0.1, 0.00, 1, 0.44); + + private RigidTransform2d zeroPose = new RigidTransform2d(new Translation2d(0,0), Rotation2d.fromDegrees(0)); + private RigidTransform2d currentPose = zeroPose; + private RigidTransform2d lastPose = zeroPose; + private double left_encoder_prev_distance_ = 0; + private double right_encoder_prev_distance_ = 0; + + //private PigeonImu gyroPigeon; + //private double[] yprPigeon = new double[3]; + private AHRS gyroNavX; + private boolean useGyroLock; + private double gyroLockAngleDeg; + //private double kPGyro = 0.04; + private double kPGyro = 0.0625; + private boolean isCalibrating = false; + private double gyroOffsetDeg = 0; + + public Drive() { + try { + leftDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, false, FeedbackDevice.QuadEncoder); + leftDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID); +// leftDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID); + + rightDrive1 = new CANTalonEncoder(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, ENCODER_TICKS_TO_INCHES, true, FeedbackDevice.QuadEncoder); + rightDrive2 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID); +// rightDrive3 = new WPI_TalonSRX(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID); + + elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + //gyroPigeon = new PigeonImu(leftDrive2); + gyroNavX = new AHRS(SPI.Port.kMXP); + + //hopperSensorRed = new DigitalInput(RobotMap.HOPPER_SENSOR_RED_DIO_ID); + //hopperSensorBlue = new DigitalInput(RobotMap.HOPPER_SENSOR_BLUE_DIO_ID); + + //leftDrive1.clearStickyFaults(); + //leftDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); + //leftDrive1.setNominalClosedLoopVoltage(12.0); + leftDrive1.clearStickyFaults(0); + leftDrive1.setInverted(false);//false on comp 2108, false on microbot + leftDrive1.setSensorPhase(true);//true on comp 2108, false on microbot + leftDrive1.setSafetyEnabled(false); + //leftDrive1.setCurrentLimit(15); + //leftDrive1.enableCurrentLimit(true); + leftDrive1.setNeutralMode(NeutralMode.Brake); + leftDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); + leftDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); + leftDrive1.configNominalOutputForward(+1.0f, 0); + leftDrive1.configNominalOutputReverse(-1.0f, 0); + + +// if (leftDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { +// Driver.reportError("Could not detect left drive encoder encoder!", false); +// } + + + leftDrive2.setInverted(false);//false on comp 2108, false on microbot + leftDrive2.setSafetyEnabled(false); + leftDrive2.setNeutralMode(NeutralMode.Brake); + leftDrive2.set(ControlMode.Follower, leftDrive1.getDeviceID()); + + + + //rightDrive1.clearStickyFaults(); + //rightDrive1.setVoltageRampRate(VOLTAGE_RAMP_RATE); + //rightDrive1.setNominalClosedLoopVoltage(12.0); + rightDrive1.clearStickyFaults(0); + rightDrive1.setInverted(true);//true on comp 2108, false on microbot + rightDrive1.setSensorPhase(true);//true on comp 2108, true on microbot + rightDrive1.setSafetyEnabled(false); + rightDrive1.setNeutralMode(NeutralMode.Brake); + rightDrive1.configOpenloopRamp(OPEN_LOOP_RAMP_SECONDS, 0); + rightDrive1.configClosedloopRamp(CLOSED_LOOP_RAMP_SECONDS, 0); + rightDrive1.configNominalOutputForward(+1.0f, 0); + rightDrive1.configNominalOutputReverse(-1.0f, 0); +// if (rightDrive1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { +// DriverStation.reportError("Could not detect right drive encoder encoder!", false); +// } + + + rightDrive2.setInverted(true);//True on comp 2108, false on microbot + rightDrive2.setSafetyEnabled(false); + rightDrive2.setNeutralMode(NeutralMode.Brake); + rightDrive2.set(ControlMode.Follower, rightDrive1.getDeviceID()); + + + + + motorControllers.add(leftDrive1); + motorControllers.add(rightDrive1); + + //m_drive = new RobotDrive(leftDrive1, rightDrive1); + //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true); + //m_drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true); + //m_drive.setSafetyEnabled(false); + m_drive = new DifferentialDrive(leftDrive1, rightDrive1); + //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearLeft, true); //TODO URGENT: verify + //m_drive.setInvertedMotor(DifferentialDrive.MotorType.kRearRight, true); //TODO URGENT: verify + m_drive.setSafetyEnabled(false); + + //speedShift = new Solenoid(RobotMap.DRIVETRAIN_SPEEDSHIFT_PCM_ID); + } + catch (Exception e) { + System.err.println("An error occurred in the DriveTrain constructor"); + } + } + + public void setToBrakeOnNeutral(boolean brakeVsCoast) { + if (brakeVsCoast) { + leftDrive1.setNeutralMode(NeutralMode.Brake); + leftDrive2.setNeutralMode(NeutralMode.Brake); + rightDrive1.setNeutralMode(NeutralMode.Brake); + rightDrive2.setNeutralMode(NeutralMode.Brake); + } else { + leftDrive1.setNeutralMode(NeutralMode.Coast); + leftDrive2.setNeutralMode(NeutralMode.Coast); + rightDrive1.setNeutralMode(NeutralMode.Coast); + rightDrive2.setNeutralMode(NeutralMode.Coast); + } + } + + @Override + public void initDefaultCommand() { + } + + public double getGyroAngleDeg() { + //return getGyroPigeonAngleDeg(); + return getGyroNavXAngleDeg(); + } + + //public double getGyroPigeonAngleDeg() { + // gyroPigeon.GetYawPitchRoll(yprPigeon); + // return -yprPigeon[0] + gyroOffsetDeg; + //} + + public double getGyroNavXAngleDeg() { + return gyroNavX.getAngle() + gyroOffsetDeg; + } + + public void resetGyro() { + //gyroPigeon.SetYaw(0); + gyroNavX.zeroYaw(); + } + + public void resetEncoders() { + mpStraightController.resetZeroPosition(); + } + + public void calibrateGyro() { + //gyroPigeon.EnterCalibrationMode(CalibrationMode.Temperature); + } + + public void endGyroCalibration() { + if (isCalibrating == true) { + isCalibrating = false; + } + } + + public void setGyroOffset(double offsetDeg) { + gyroOffsetDeg = offsetDeg; + } + + //public boolean isHopperSensorRedOn() { + // return hopperSensorRed.get(); + //} + + //public boolean isHopperSensorBlueOn() { + // return hopperSensorBlue.get(); + //} + + //public boolean isHopperSensorOn() { + // if (isRed() == true) { + // return isHopperSensorRedOn(); + // } + // else { + // return isHopperSensorBlueOn(); + // } + //} + + public void setStraightMM(double distanceInches, double maxVelocity, double maxAcceleration, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); + mmStraightController.setPID(mmStraightPIDParams); + mmStraightController.setMMStraightTarget(0, distanceInches, maxVelocity, maxAcceleration, useGyroLock, yawAngle, true); + setControlMode(DriveControlMode.MOTION_MAGIC); + } + + public void setStraightMP(double distanceInches, double maxVelocity, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); + mpStraightController.setPID(mpStraightPIDParams); + mpStraightController.setMPStraightTarget(0, distanceInches, maxVelocity, MP_STRAIGHT_T1, MP_STRAIGHT_T2, useGyroLock, yawAngle, true); + setControlMode(DriveControlMode.MP_STRAIGHT); + } + + //public void setStraightMPCached(String key, boolean useGyroLock, boolean useAbsolute, double desiredAbsoluteAngle) { + // double yawAngle = useAbsolute ? BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), desiredAbsoluteAngle) : getGyroAngleDeg(); + // mpStraightController.setPID(mpStraightPIDParams); + // mpStraightController.setMPStraightTarget(key, useGyroLock, yawAngle, true); + // setControlMode(DriveControlMode.MP_STRAIGHT); + //} + + public void setRelativeTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { + mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); + setControlMode(DriveControlMode.MP_TURN); + } + + //public void setRelativeTurnMPCached(String key, MPSoftwareTurnType turnType) { + // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); + // setControlMode(DriveControlMode.MP_TURN); + //} + + public void setRelativeMaxTurnMP(double relativeTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { + mpTurnController.setMPTurnTarget(getGyroAngleDeg(), relativeTurnAngleDeg + getGyroAngleDeg(), turnRateDegPerSec, MP_MAX_TURN_T1, MP_MAX_TURN_T2, turnType, TRACK_WIDTH_INCHES); + setControlMode(DriveControlMode.MP_TURN); + } + + public void setAbsoluteTurnMP(double absoluteTurnAngleDeg, double turnRateDegPerSec, MPSoftwareTurnType turnType) { + mpTurnController.setMPTurnTarget(getGyroAngleDeg(), BHRMathUtils.adjustAccumAngleToDesired(getGyroAngleDeg(), absoluteTurnAngleDeg), turnRateDegPerSec, MP_TURN_T1, MP_TURN_T2, turnType, TRACK_WIDTH_INCHES); + setControlMode(DriveControlMode.MP_TURN); + } + + //public void setAbsoluteTurnMPCached(String key, MPSoftwareTurnType turnType) { + // mpTurnController.setMPTurnTarget(key, turnType, TRACK_WIDTH_INCHES); + // setControlMode(DriveControlMode.MP_TURN); + //} + + public void setRelativeTurnPID(double relativeTurnAngleDeg, double maxError, double maxPrevError, MPSoftwareTurnType turnType) { + this.targetPIDAngle = relativeTurnAngleDeg + getGyroAngleDeg(); + pidTurnController.setPIDTurnTarget(relativeTurnAngleDeg + getGyroAngleDeg(), maxError, maxPrevError, turnType); + setControlMode(DriveControlMode.PID_TURN); + } + + public void setPathMP(PathGenerator path) { + mpPathController.setPID(mpPathPIDParams); + mpPathController.setMPPathTarget(path); + setControlMode(DriveControlMode.MP_PATH); + } + + public void setPathVelocityMP(PathGenerator path) { + mpPathVelocityController.setPID(mpPathPIDParams); + mpPathVelocityController.setMPPathTarget(path); + setControlMode(DriveControlMode.MP_PATH_VELOCITY); + } + + public void setPathAdaptivePursuit(Path path, boolean reversed) { + currentPose = zeroPose; + lastPose = zeroPose; + left_encoder_prev_distance_ = 0; + right_encoder_prev_distance_ = 0; + adaptivePursuitController.setPID(adaptivePursuitPIDParams); + adaptivePursuitController.setPath(Constants.kPathFollowingLookahead, Constants.kPathFollowingMaxAccel, path, reversed, 0.25); + setControlMode(DriveControlMode.ADAPTIVE_PURSUIT); + } + + public void setDriveHold(boolean status) { + if (status) { + setControlMode(DriveControlMode.HOLD); + } + else { + setControlMode(DriveControlMode.JOYSTICK); + } + } + + public void updatePose() { + double left_distance = leftDrive1.getPositionWorld(); + double right_distance = rightDrive1.getPositionWorld(); + Rotation2d gyro_angle = Rotation2d.fromDegrees(-getGyroAngleDeg()); + lastPose = currentPose; + currentPose = generateOdometryFromSensors(left_distance - left_encoder_prev_distance_, right_distance - right_encoder_prev_distance_, gyro_angle); + left_encoder_prev_distance_ = left_distance; + right_encoder_prev_distance_ = right_distance; + } + + public RigidTransform2d generateOdometryFromSensors(double left_encoder_delta_distance, double right_encoder_delta_distance, Rotation2d current_gyro_angle) { + return Kinematics.integrateForwardKinematics(lastPose, left_encoder_delta_distance, right_encoder_delta_distance, current_gyro_angle); + } + + /** + * Path Markers are an optional functionality that name the various + * Waypoints in a Path with a String. This can make defining set locations + * much easier. + * + * @return Set of Strings with Path Markers that the robot has crossed. + */ + public synchronized Set getPathMarkersCrossed() { + if (controlMode != DriveControlMode.ADAPTIVE_PURSUIT) { + return null; + } else { + return adaptivePursuitController.getMarkersCrossed(); + } + } + + public synchronized void setControlMode(DriveControlMode controlMode) { + this.controlMode = controlMode; + if (controlMode == DriveControlMode.JOYSTICK) { + //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); + //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); + leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + } + else if (controlMode == DriveControlMode.MANUAL) { + //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); + //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); + leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + } + else if (controlMode == DriveControlMode.CLIMB) { + //leftDrive1.changeControlMode(TalonControlMode.PercentVbus); + //rightDrive1.changeControlMode(TalonControlMode.PercentVbus); + leftDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + rightDrive1.set(ControlMode.PercentOutput, 0); //TODO URGENT: make sure not called when robot moving + } + else if (controlMode == DriveControlMode.HOLD) { + mpStraightController.setPID(mpHoldPIDParams); + //leftDrive1.changeControlMode(TalonControlMode.Position); + //leftDrive1.setPosition(0); + //leftDrive1.set(0); + //rightDrive1.changeControlMode(TalonControlMode.Position); + //rightDrive1.setPosition(0); + //rightDrive1.set(0); + leftDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + leftDrive1.set(ControlMode.Position, 0); + rightDrive1.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + rightDrive1.set(ControlMode.Position, 0); + } + isFinished = false; + } + + public synchronized void controlLoopUpdate() { + // Measure *actual* update period + double currentTimestamp = Timer.getFPGATimestamp(); + if (lastControlLoopUpdateTimestamp > 0.001) { // ie, if this is NOT the first time + lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; + } + lastControlLoopUpdateTimestamp = currentTimestamp; + + // Do the update + if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) { + driveWithJoystick(); + } + else if (!isFinished) { + if (controlMode == DriveControlMode.MP_STRAIGHT) { + isFinished = mpStraightController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.MP_TURN) { + isFinished = mpTurnController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.PID_TURN) { + isFinished = pidTurnController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.MP_PATH) { + isFinished = mpPathController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.MP_PATH_VELOCITY) { + isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.MOTION_MAGIC) { + isFinished = mmStraightController.controlLoopUpdate(getGyroAngleDeg()); + } + else if (controlMode == DriveControlMode.ADAPTIVE_PURSUIT) { + updatePose(); + isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); + } + } + } + + public void setSpeed(double speed) { + if (speed == 0) { + setControlMode(DriveControlMode.JOYSTICK); + } + else { + setControlMode(DriveControlMode.MANUAL); + rightDrive1.set(-speed); + leftDrive1.set(speed); + } + } + + public void setClimbSpeed(double climbSpeed) { + this.climbSpeed = climbSpeed; + if (climbSpeed == 0) { + setControlMode(DriveControlMode.JOYSTICK); + } + else { + setControlMode(DriveControlMode.CLIMB); + } + } + + public void setGyroLock(boolean useGyroLock, boolean snapToAbsolute0or180) { + if (snapToAbsolute0or180) { + gyroLockAngleDeg = BHRMathUtils.adjustAccumAngleToClosest180(getGyroAngleDeg()); + } + else { + gyroLockAngleDeg = getGyroAngleDeg(); + } + this.useGyroLock = useGyroLock; + } + + public void driveWithJoystick() { + if(m_drive == null) return; + // switch(m_controllerMode) { + // case CONTROLLER_JOYSTICK_ARCADE: + // m_moveInput = OI.getInstance().getJoystick1().getY(); + // m_steerInput = OI.getInstance().getJoystick1().getX(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); + // break; + // case CONTROLLER_JOYSTICK_TANK: + // m_moveInput = OI.getInstance().getJoystick1().getY(); + // m_steerInput = OI.getInstance().getJoystick2().getY(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_steerInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_drive.tankDrive(m_moveOutput, m_steerOutput); + // break; + // case CONTROLLER_JOYSTICK_CHEESY: + // m_moveInput = OI.getInstance().getJoystick1().getY(); + // m_steerInput = OI.getInstance().getJoystick2().getX(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); + // break; + // case CONTROLLER_XBOX_CHEESY: + // boolean turbo = OI.getInstance().getDriveTrainController() + // .getLeftJoystickButton(); + // boolean slow = OI.getInstance().getDriveTrainController() + // .getRightJoystickButton(); + // double speedToUseMove, speedToUseSteer; + // if (turbo && !slow) { + // speedToUseMove = m_moveScaleTurbo; + // speedToUseSteer = m_steerScaleTurbo; + // } else if (!turbo && slow) { + // speedToUseMove = m_moveScaleSlow; + // speedToUseSteer = m_steerScaleSlow; + // } else { + // speedToUseMove = m_moveScale; + // speedToUseSteer = m_steerScale; + // } + + // m_moveInput = + // OI.getInstance().getDriveTrainController().getLeftYAxis(); + // m_steerInput = + // OI.getInstance().getDriveTrainController().getRightXAxis(); + m_moveInput = -OI.getInstance().getDriverController().getLeftYAxis(); + m_steerInput = OI.getInstance().getDriverController().getRightXAxis(); + + if (controlMode == DriveControlMode.JOYSTICK) { + m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + } + else if (controlMode == DriveControlMode.CLIMB) { + m_moveOutput = climbSpeed; + } + + if (useGyroLock) { // If currently in gyro lock mode, + if ((m_moveInput == 0.0) || (m_steerInput != 0.0)) { // but stopped driving or started turning, + setGyroLock(false, false); // turn off gyro lock mode + } + } else { // If not yet in gyro lock mode, + if ((m_moveInput != 0.0) && (m_steerInput == 0.0)) { // but just started driving without turning, + setGyroLock(true, false); // gyro lock to current heading + } + } + + if (useGyroLock) { + double yawError = gyroLockAngleDeg - getGyroAngleDeg(); + m_steerOutput = kPGyro * yawError; + } else { + m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + } + + m_drive.arcadeDrive(m_moveOutput, m_steerOutput*.75); + // break; + // case CONTROLLER_XBOX_ARCADE_RIGHT: + // m_moveInput = + // OI.getInstance().getDrivetrainController().getRightYAxis(); + // m_steerInput = + // OI.getInstance().getDrivetrainController().getRightXAxis(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); + // break; + // case CONTROLLER_XBOX_ARCADE_LEFT: + // m_moveInput = + // OI.getInstance().getDrivetrainController().getLeftYAxis(); + // m_steerInput = + // OI.getInstance().getDrivetrainController().getLeftXAxis(); + // m_moveOutput = adjustForSensitivity(m_moveScale, m_moveTrim, + // m_moveInput, m_moveNonLinear, MOVE_NON_LINEARITY); + // m_steerOutput = adjustForSensitivity(m_steerScale, m_steerTrim, + // m_steerInput, m_steerNonLinear, STEER_NON_LINEARITY); + // m_drive.arcadeDrive(m_moveOutput, m_steerOutput); + // break; + // } + } + + public void rawMoveSteer(double move, double steer) { + m_drive.arcadeDrive(move, steer, false); + } + + public void rawDriveLeftRight(double leftPercentOutput, double rightPercentOutput) { + + if (elevatorRight.getSelectedSensorPosition(0) >= 3550) { + leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput*.5); + rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput*.5); + } + else /*(elevatorRight.getSelectedSensorPosition(0) < 3550)*/ { + leftDrive1.set(ControlMode.PercentOutput, leftPercentOutput); + rightDrive1.set(ControlMode.PercentOutput, rightPercentOutput); + } + } + + private boolean inDeadZone(double input) { + boolean inDeadZone; + if (Math.abs(input) < STICK_DEADBAND) { + inDeadZone = true; + } else { + inDeadZone = false; + } + return inDeadZone; + } + + public double adjustForSensitivity(double scale, double trim, + double steer, int nonLinearFactor, double wheelNonLinearity) { + if (inDeadZone(steer)) + return 0; + + steer += trim; + steer *= scale; + steer = limitValue(steer); + + int iterations = Math.abs(nonLinearFactor); + for (int i = 0; i < iterations; i++) { + if (nonLinearFactor > 0) { + steer = nonlinearStickCalcPositive(steer, wheelNonLinearity); + } else { + steer = nonlinearStickCalcNegative(steer, wheelNonLinearity); + } + } + return steer; + } + + private double limitValue(double value) { + if (value > 1.0) { + value = 1.0; + } else if (value < -1.0) { + value = -1.0; + } + return value; + } + + private double nonlinearStickCalcPositive(double steer, + double steerNonLinearity) { + return Math.sin(Math.PI / 2.0 * steerNonLinearity * steer) + / Math.sin(Math.PI / 2.0 * steerNonLinearity); + } + + private double nonlinearStickCalcNegative(double steer, + double steerNonLinearity) { + return Math.asin(steerNonLinearity * steer) + / Math.asin(steerNonLinearity); + } + + //public void setShiftState(SpeedShiftState state) { + // if(state == SpeedShiftState.HI) { + // speedShift.set(true); + // } + // else if(state == SpeedShiftState.LO) { + // speedShift.set(false); + // } + //} + + public synchronized boolean isFinished() { + return isFinished; + } + + public synchronized void setFinished(boolean isFinished) { + this.isFinished = isFinished; + } + + @Override + public void setPeriodMs(long periodMs) { + //mmStraightController = new MMTalonPIDController(periodMs, mmStraightPIDParams, motorControllers); + mpStraightController = new MPTalonPIDController(periodMs, mpStraightPIDParams, motorControllers); + mpTurnController = new MPSoftwarePIDController(periodMs, mpTurnPIDParams, motorControllers); + pidTurnController = new SoftwarePIDController(pidTurnPIDParams, motorControllers); + mpPathController = new MPTalonPIDPathController(periodMs, mpPathPIDParams, motorControllers); + mpPathVelocityController = new MPTalonPIDPathVelocityController(periodMs, mpPathVelocityPIDParams, motorControllers); + adaptivePursuitController = new AdaptivePurePursuitController(periodMs, adaptivePursuitPIDParams, motorControllers); + + this.periodMs = periodMs; + } + + public double getPeriodMs() { + return periodMs; + } + + public boolean isRed() { + return isRed; + } + + public void setIsRed(boolean status) { + isRed = status; + } + + public static double rotationsToInches(double rotations) { + return rotations * (Constants.kDriveWheelDiameterInches * Math.PI); + } + + public static double rpmToInchesPerSecond(double rpm) { + return rotationsToInches(rpm) / 60; + } + + public static double inchesToRotations(double inches) { + return inches / (Constants.kDriveWheelDiameterInches * Math.PI); + } + + public static double inchesPerSecondToRpm(double inches_per_second) { + return inchesToRotations(inches_per_second) * 60; + } + + public double getLeftPositionWorld() { + return leftDrive1.getPositionWorld(); + } + + public double getRightPositionWorld() { + return rightDrive1.getPositionWorld(); + } + + public void updateStatus(Robot.OperationMode operationMode) { + if (operationMode == Robot.OperationMode.TEST) { + try { + SmartDashboard.putNumber("Update Period (ms)", lastControlLoopUpdatePeriod * 1000.0); + SmartDashboard.putNumber("Right Pos Ticks", rightDrive1.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Left Pos Ticks", leftDrive1.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Right Pos Inches", rightDrive1.getPositionWorld()); + SmartDashboard.putNumber("Left Pos Inches", leftDrive1.getPositionWorld()); + SmartDashboard.putNumber("Right Vel Ft-Sec", rightDrive1.getVelocityWorld() / 12); + SmartDashboard.putNumber("Left Vel Ft-Sec", leftDrive1.getVelocityWorld() / 12); + //SmartDashboard.putNumber("Left 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID)); + //SmartDashboard.putNumber("Left 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID)); +// SmartDashboard.putNumber("Left 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_LEFT_MOTOR3_CAN_ID)); + //SmartDashboard.putNumber("Right 1 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID)); + //SmartDashboard.putNumber("Right 2 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID)); +// SmartDashboard.putNumber("Right 3 Amps", Robot.pdp.getCurrent(RobotMap.DRIVETRAIN_RIGHT_MOTOR3_CAN_ID)); + //SmartDashboard.putBoolean("Hopper Sensor Red", isHopperSensorRedOn()); + //SmartDashboard.putBoolean("Hopper Sensor Blue", isHopperSensorBlueOn()); + SmartDashboard.putBoolean("Drive Hold", controlMode == DriveControlMode.HOLD); + //SmartDashboard.putNumber("Yaw Angle Pigeon Deg", getGyroPigeonAngleDeg()); + SmartDashboard.putNumber("Yaw Angle Deg", getGyroAngleDeg()); + MotionProfilePoint mpPoint = mpTurnController.getCurrentPoint(); + double delta = mpPoint != null ? getGyroAngleDeg() - mpTurnController.getCurrentPoint().position : 0; + SmartDashboard.putNumber("Gyro Delta", delta); + //SmartDashboard.putBoolean("Gyro Calibrating", isCalibrating); + SmartDashboard.putBoolean("Gyro Calibrating", gyroNavX.isCalibrating()); + SmartDashboard.putNumber("Gyro Offset", gyroOffsetDeg); + SmartDashboard.putNumber("Delta PID Angle", targetPIDAngle - getGyroAngleDeg()); + SmartDashboard.putNumber("Steer Output", m_steerOutput); + SmartDashboard.putNumber("Move Output", m_moveOutput); + SmartDashboard.putNumber("Steer Input", m_steerInput); + SmartDashboard.putNumber("Move Input", m_moveInput); + } + catch (Exception e) { + System.err.println("Drivetrain update status error"); + } + } + else { + + } + } + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java new file mode 100644 index 0000000..4a17e98 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Elevator.java @@ -0,0 +1,450 @@ +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 Elevator extends Subsystem implements ControlLoopable +{ + //PID encoder and motor + private CANTalonEncoder elevatorRight; + private WPI_TalonSRX elevatorLeft; + + //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.kElevatorEncoderTicksPerInch; + + //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 elevatorControlMode = false; + // Motor controllers + //private ArrayList motorControllers = new ArrayList(); + + public Elevator() + { + try + { + //PID elevator encoder and talon + elevatorRight = new CANTalonEncoder(RobotMap.ELEVATOR_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + elevatorLeft = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR2_ID); + + elevatorRight.setInverted(false); + + //Setting left elevator motor as follower + elevatorLeft.set(ControlMode.Follower, elevatorRight.getDeviceID()); + elevatorLeft.setInverted(false); + elevatorLeft.setNeutralMode(NeutralMode.Brake); + elevatorRight.setNeutralMode(NeutralMode.Brake); + elevatorRight.setSensorPhase(true); + //Limit Switch Left + //elevatorLeft.overrideLimitSwitchesEnable(true); + elevatorLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + elevatorLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + + //Limit Switch Right + //elevatorRight.overrideLimitSwitchesEnable(true); + //elevatorRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //elevatorRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + + + //Change This boi + + // elevatorRight.configForwardSoftLimitThreshold(10000, 0); //right here + //elevatorRight.configReverseSoftLimitThreshold(5, 0); + //elevatorRight.configForwardSoftLimitEnable(true, 0); + //elevatorRight.configReverseSoftLimitEnable(true, 0); + + //sos + //elevatorRight.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 Elevator 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 setElevatorMode() + { + setControlMode(DriveControlMode.JOYSTICK); + } + + public void resetElevatorEncoder() + { + elevatorRight.setSelectedSensorPosition(0, 0, 0); + } + + public void moveElevatorXbox() + { + double moveElevatorInput; + double elevatorSafeZone = 15; + + double elevatorPos = getEncoderElevatorPosition(); + + moveElevatorInput = Robot.oi.getOperatorController().getLeftYAxis(); + + //double moveElevatorSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveElevatorInput, moveNonLinear, MOVE_NON_LINEARITY); + + boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); + boolean elevatorTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); + + if(elevatorTuningPressed == true) + { + elevatorRight.set(moveElevatorInput * 0.5); + } + else if(elevatorTuningPressed == false) + { + elevatorRight.set(moveElevatorInput); + } + + /* + if(elevatorPos <= elevatorSafeZone && elevatorPos >= 0) + { + elevatorRight.set(moveElevatorInput); + } + else if(elevatorPos > elevatorSafeZone) + { + elevatorRight.set(moveElevatorInput * 0.65); + + + if(holdButtonPressed == true) + { + elevatorRight.set(-0.43 * (0.2)); + } + else if(holdButtonPressed == false) + { + elevatorRight.set(moveElevatorInput * 0.75); + } + + } + + else if(elevatorPos < 0) + { + elevatorRight.set(moveElevatorInput * 0.75); + } + */ + } + + +// System.out.println(elevatorPos); //-6.9 to 1.9 total: 8.8 range + + + //PID encoder position + public double getEncoderElevatorPosition() + { + return elevatorRight.getPositionWorld(); + } + + public double getElevatorHeightInchesAboveFloor() + { + return elevatorRight.getPositionWorld(); + } + + public synchronized void setControlMode(DriveControlMode controlMode) + { + this.controlMode = controlMode; + + isFinished = false; + } + /* + public void setElevatorPIDMaxScale(double ElevatorPosition, double maxError, double minError) + { + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); + } + + public void setElevatorPIDLowScale(double ElevatorPosition, double maxError, double minError) + { + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); + } + + public void setElevatorPIDSwitch(double ElevatorPosition, double maxError, double minError) + { + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); + } + + public void setElevatorPIDLowest(double ElevatorPosition, double maxError, double minError) + { + double elevatorTargetPos = 0; + this.targetPPosition = elevatorTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(elevatorTargetPos, maxError, minError); + Robot.elevator.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); + } + */ + public void rawSetOutput(double output){ + elevatorRight.set(/*ControlMode.PercentOutput,*/ output); + } + + public void holdInPos() + { + elevatorRight.set(-0.43 * 0.2); + } + + public void stopMotors() + { + elevatorRight.set(0); + } + + public void isSwitchPressed() + { + pressed = false; + isPressed = elevatorRight.getSensorCollection(); + + if(isPressed.isFwdLimitSwitchClosed() == true) + { + if (controlMode == DriveControlMode.JOYSTICK) { + Robot.elevator.setControlMode(DriveControlMode.STOP_MOTORS); + } + pressed = true; + } + else + { + if (controlMode == DriveControlMode.STOP_MOTORS){ + { + Robot.elevator.setControlMode(DriveControlMode.JOYSTICK); + } + + pressed = false; + } + } + + } + + //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; + + + + + + @Override + public void controlLoopUpdate() + { + if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) + { + moveElevatorXbox(); + } + else if (!isFinished) + { + //PID control mode + if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE) + { + isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderElevatorPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE) + { + isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderElevatorPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH) + { + isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderElevatorPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST) + { + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); + } + /* + else if(controlMode == DriveControlMode.RAW) + { + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderElevatorPosition()); + } + */ + } + } + + @Override + public void initDefaultCommand() + { + } + + @Override + public void periodic() + { + // isSwitchPressed(); + } + + @Override + public void setPeriodMs(long periodMs) + { + //PID controller + pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, elevatorRight); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, elevatorRight); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, elevatorRight); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, elevatorRight); + + 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("Elevator Pos Ticks", elevatorRight.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Elevator Pos Inches", getElevatorHeightInchesAboveFloor()); + //SmartDashboard.putData(pressed); + } + catch (Exception e) + { + System.err.println("Drivetrain update status error"); + } + } + } + + +} + diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java new file mode 100644 index 0000000..129b561 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Pnumatics.java @@ -0,0 +1,46 @@ +package org.usfirst.frc4388.robot.subsystems; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.RobotMap; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.command.Subsystem; + +public class Pnumatics extends Subsystem { + + + private DoubleSolenoid speedShift; + private DoubleSolenoid Intake; + + + public Pnumatics() { + try { + speedShift = new DoubleSolenoid(1,0,1); + Intake = new DoubleSolenoid(1,4,5 ); + } + catch (Exception e) { + System.err.println("An error occurred in the Pnumatics constructor"); + } + } + + public void setShiftState(boolean state) { + if (state==true) { + speedShift.set(DoubleSolenoid.Value.kForward); + } + if (state==false) { + speedShift.set(DoubleSolenoid.Value.kReverse); + } + } + public void setIntake(boolean state) { + if (state==true) { + Intake.set(DoubleSolenoid.Value.kForward); + } + if (state==false) { + Intake.set(DoubleSolenoid.Value.kReverse); + } + } + + public void initDefaultCommand() { + } +} + diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java new file mode 100644 index 0000000..c217c81 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/AdaptivePurePursuitController.java @@ -0,0 +1,228 @@ +package org.usfirst.frc4388.utility; + +import java.util.ArrayList; +import java.util.Optional; +import java.util.Set; + +import org.usfirst.frc4388.robot.Constants; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.Timer; + +/** + * Implements an adaptive pure pursuit controller. See: + * https://www.ri.cmu.edu/pub_files/pub1/kelly_alonzo_1994_4/kelly_alonzo_1994_4 + * .pdf + * + * Basically, we find a spot on the path we'd like to follow and calculate the + * wheel speeds necessary to make us land on that spot. The target spot is a + * specified distance ahead of us, and we look further ahead the greater our + * tracking error. + */ +public class AdaptivePurePursuitController { + private static final double kEpsilon = 1E-9; + + double mFixedLookahead; + Path mPath; + RigidTransform2d.Delta mLastCommand; + double mLastTime; + double mMaxAccel; + double mDt; + boolean mReversed; + double mPathCompletionTolerance; + + protected ArrayList motorControllers; + protected long periodMs; + protected PIDParams pidParams; + protected double startGyroAngle; + protected double targetGyroAngle; + protected double trackDistance; + + public AdaptivePurePursuitController(long periodMs, PIDParams pidParams, ArrayList motorControllers) + { + this.motorControllers = motorControllers; + this.periodMs = periodMs; + setPID(pidParams); + } + + public void setPID(PIDParams pidParams) { + this.pidParams = pidParams; + + for (CANTalonEncoder motorController : motorControllers) { + //motorController.setPID(pidParams.kP, pidParams.kI, pidParams.kD); + //motorController.setF(pidParams.kF); + //motorController.configNominalOutputVoltage(+0.0f, -0.0f); + //motorController.configPeakOutputVoltage(+12.0f, -12.0f); + //motorController.setProfile(0); + motorController.config_kP(0, pidParams.kP, 0); + motorController.config_kI(0, pidParams.kI, 0); + motorController.config_kD(0, pidParams.kD, 0); + motorController.config_kF(0, pidParams.kF, 0); + motorController.configNominalOutputForward(+0.0f, 0); + motorController.configNominalOutputReverse(-0.0f, 0); + motorController.configPeakOutputForward(+1.0f, 0); + motorController.configPeakOutputReverse(-1.0f, 0); + motorController.selectProfileSlot(0, 0); + } + } + + public void setPath(double fixed_lookahead, double max_accel, Path path, + boolean reversed, double path_completion_tolerance) { + mFixedLookahead = fixed_lookahead; + mMaxAccel = max_accel; + mPath = path; + mDt = periodMs; + mLastCommand = null; + mReversed = reversed; + mPathCompletionTolerance = path_completion_tolerance; + + // Set up the motion profile + for (CANTalonEncoder motorController : motorControllers) { + //motorController.setPosition(0); + //motorController.set(0); + //motorController.changeControlMode(TalonControlMode.Speed); + motorController.setSelectedSensorPosition(0, 0, 0); //TODO: verify want 0="Primary closed-loop", with no timeout + motorController.set(ControlMode.Velocity, 0); + } + } + + public boolean isDone() { + double remainingLength = mPath.getRemainingLength(); + return remainingLength <= mPathCompletionTolerance; + } + + public boolean controlLoopUpdate(RigidTransform2d robot_pose) { + RigidTransform2d.Delta command = update(robot_pose, Timer.getFPGATimestamp()); + Kinematics.DriveVelocity setpoint = Kinematics.inverseKinematics(command); + + // Scale the command to respect the max velocity limits + double max_vel = 0.0; + max_vel = Math.max(max_vel, Math.abs(setpoint.left)); + max_vel = Math.max(max_vel, Math.abs(setpoint.right)); + if (max_vel > Constants.kPathFollowingMaxVel) { + double scaling = Constants.kPathFollowingMaxVel / max_vel; + setpoint = new Kinematics.DriveVelocity(setpoint.left * scaling, setpoint.right * scaling); + } + + // Update the controllers Kf and set point. + for (CANTalonEncoder motorController : motorControllers) { + if (motorController.isRight()) { + motorController.setVelocityWorld(-setpoint.right); + } + else { + motorController.setVelocityWorld(-setpoint.left); + } + } + + return isDone(); + } + + public RigidTransform2d.Delta update(RigidTransform2d robot_pose, double now) { + RigidTransform2d pose = robot_pose; + if (mReversed) { + pose = new RigidTransform2d(robot_pose.getTranslation(), + robot_pose.getRotation().rotateBy(Rotation2d.fromRadians(Math.PI))); + } + + double distance_from_path = mPath.update(robot_pose.getTranslation()); + if (this.isDone()) { + return new RigidTransform2d.Delta(0, 0, 0); + } + + PathSegment.Sample lookahead_point = mPath.getLookaheadPoint(robot_pose.getTranslation(), + distance_from_path + mFixedLookahead); + Optional circle = joinPath(pose, lookahead_point.translation); + + double speed = lookahead_point.speed; + if (mReversed) { + speed *= -1; + } + // Ensure we don't accelerate too fast from the previous command + double dt = now - mLastTime; + if (mLastCommand == null) { + mLastCommand = new RigidTransform2d.Delta(0, 0, 0); + dt = mDt; + } + double accel = (speed - mLastCommand.dx) / dt; + if (accel < -mMaxAccel) { + speed = mLastCommand.dx - mMaxAccel * dt; + } else if (accel > mMaxAccel) { + speed = mLastCommand.dx + mMaxAccel * dt; + } + + // Ensure we slow down in time to stop + // vf^2 = v^2 + 2*a*d + // 0 = v^2 + 2*a*d + double remaining_distance = mPath.getRemainingLength(); + double max_allowed_speed = Math.sqrt(2 * mMaxAccel * remaining_distance); + if (Math.abs(speed) > max_allowed_speed) { + speed = max_allowed_speed * Math.signum(speed); + } + final double kMinSpeed = 4.0; + if (Math.abs(speed) < kMinSpeed) { + // Hack for dealing with problems tracking very low speeds with + // Talons + speed = kMinSpeed * Math.signum(speed); + } + + RigidTransform2d.Delta rv; + if (circle.isPresent()) { + rv = new RigidTransform2d.Delta(speed, 0, + (circle.get().turn_right ? -1 : 1) * Math.abs(speed) / circle.get().radius); + } else { + rv = new RigidTransform2d.Delta(speed, 0, 0); + } + mLastTime = now; + mLastCommand = rv; + return rv; + } + + public Set getMarkersCrossed() { + return mPath.getMarkersCrossed(); + } + + public static class Circle { + public final Translation2d center; + public final double radius; + public final boolean turn_right; + + public Circle(Translation2d center, double radius, boolean turn_right) { + this.center = center; + this.radius = radius; + this.turn_right = turn_right; + } + } + + public static Optional joinPath(RigidTransform2d robot_pose, Translation2d lookahead_point) { + double x1 = robot_pose.getTranslation().getX(); + double y1 = robot_pose.getTranslation().getY(); + double x2 = lookahead_point.getX(); + double y2 = lookahead_point.getY(); + + Translation2d pose_to_lookahead = robot_pose.getTranslation().inverse().translateBy(lookahead_point); + double cross_product = pose_to_lookahead.getX() * robot_pose.getRotation().sin() + - pose_to_lookahead.getY() * robot_pose.getRotation().cos(); + if (Math.abs(cross_product) < kEpsilon) { + return Optional.empty(); + } + + double dx = x1 - x2; + double dy = y1 - y2; + double my = (cross_product > 0 ? -1 : 1) * robot_pose.getRotation().cos(); + double mx = (cross_product > 0 ? 1 : -1) * robot_pose.getRotation().sin(); + + double cross_term = mx * dx + my * dy; + + if (Math.abs(cross_term) < kEpsilon) { + // Points are colinear + return Optional.empty(); + } + + return Optional.of(new Circle( + new Translation2d((mx * (x1 * x1 - x2 * x2 - dy * dy) + 2 * my * x1 * dy) / (2 * cross_term), + (-my * (-y1 * y1 + y2 * y2 + dx * dx) + 2 * mx * y1 * dx) / (2 * cross_term)), + .5 * Math.abs((dx * dx + dy * dy) / cross_term), cross_product > 0)); + } + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java new file mode 100644 index 0000000..8d3fc74 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/BHRMathUtils.java @@ -0,0 +1,48 @@ +package org.usfirst.frc4388.utility; + +public class BHRMathUtils { + + public static double normalizeAngle0To360(double currentAccumAngle) { + // reduce the angle + double normalizedAngle = currentAccumAngle % 360; + + // force it to be the positive remainder, so that 0 <= angle < 360 + normalizedAngle = (normalizedAngle + 360) % 360; + + return normalizedAngle; + } + + public static double adjustAccumAngleToDesired(double currentAccumAngle, double desiredAngle0To360) { + double normalizedAngle = normalizeAngle0To360(currentAccumAngle); + + if ( Math.abs(normalizedAngle - desiredAngle0To360) <= 180) { + double deltaAngle = normalizedAngle - desiredAngle0To360; + return currentAccumAngle - deltaAngle; + } + else { + double deltaAngle = desiredAngle0To360 - normalizedAngle; + if (deltaAngle < 0) { + deltaAngle += 360; + } + else { + deltaAngle -= 360; + } + return currentAccumAngle + deltaAngle; + } + } + + public static double adjustAccumAngleToClosest180(double currentAccumAngle) { + double normalizedAngle = Math.abs(normalizeAngle0To360(currentAccumAngle)); + + if (normalizedAngle < 90 || normalizedAngle > 270) { + return adjustAccumAngleToDesired(currentAccumAngle, 0); + } + else { + return adjustAccumAngleToDesired(currentAccumAngle, 180); + } + } + + public static void main(String[] args) { + System.out.println("Accum angle to desired 721 to 45 = " + adjustAccumAngleToDesired(721, 45)); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java new file mode 100644 index 0000000..52088f7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java @@ -0,0 +1,65 @@ +package org.usfirst.frc4388.utility; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +public class CANTalonEncoder extends WPI_TalonSRX +{ + private double encoderTicksToWorld; + private boolean isRight = true; + + + + public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) { + this(deviceNumber, encoderTicksToWorld, false, feedbackDevice); + } + + public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) { + super(deviceNumber); + //this.setFeedbackDevice(feedbackDevice); + this.configSelectedFeedbackSensor(feedbackDevice, 0, 0); + this.encoderTicksToWorld = encoderTicksToWorld; + this.isRight = isRight; + } + + public boolean isRight() { + return isRight; + } + + public void setRight(boolean isRight) { + this.isRight = isRight; + } + + public double convertEncoderTicksToWorld(double encoderTicks) { + return encoderTicks / encoderTicksToWorld; + } + + public double convertEncoderWorldToTicks(double worldValue) { + return worldValue * encoderTicksToWorld; + } + + public void setWorld(double worldValue) { + //this.set(convertEncoderWorldToTicks(worldValue)); + this.set(getControlMode(), convertEncoderWorldToTicks(worldValue)); //TODO: change to explicit mode set? + } + + public void setPositionWorld(double worldValue) { + //this.setPosition(convertEncoderWorldToTicks(worldValue)); + this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0); //TODO: verify + } + + public double getPositionWorld() { + //return convertEncoderTicksToWorld(this.getPosition()); + return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0)); //TODO: verify want 0="Primary closed-loop" + } + + public void setVelocityWorld(double worldValue) { + //this.set(convertEncoderWorldToTicks(worldValue) * 0.1); + this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1); //TODO: change to explicit mode set? + } + + public double getVelocityWorld() { + //return convertEncoderTicksToWorld(this.getSpeed() / 0.1); + return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1); //TODO: verify want 0="Primary closed-loop" + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java new file mode 100644 index 0000000..135bb97 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ConstantsBase.java @@ -0,0 +1,199 @@ +package org.usfirst.frc4388.utility; + +import java.io.File; +import java.io.FileReader; +import java.io.FileWriter; +import java.io.IOException; +import java.lang.reflect.Field; +import java.math.BigDecimal; +import java.util.ArrayList; +import java.util.Collection; +import java.util.HashMap; +import java.util.List; +import java.util.Set; + +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; +import org.json.simple.parser.ParseException; + +/** + * ConstantsBase + * + * Base class for storing robot constants. Anything stored as a public static + * field will be reflected and be able to set externally + */ +public abstract class ConstantsBase { + HashMap modifiedKeys = new HashMap(); + + public abstract String getFileLocation(); + + public static class Constant { + public String name; + public Class type; + public Object value; + + public Constant(String name, Class type, Object value) { + this.name = name; + this.type = type; + this.value = value; + } + + @Override + public boolean equals(Object o) { + String itsName = ((Constant) o).name; + Class itsType = ((Constant) o).type; + Object itsValue = ((Constant) o).value; + return o instanceof Constant && this.name.equals(itsName) && this.type.equals(itsType) + && this.value.equals(itsValue); + } + } + + public File getFile() { + String filePath = getFileLocation(); + filePath = filePath.replaceFirst("^~", System.getProperty("user.home")); + return new File(filePath); + } + + public boolean setConstant(String name, Double value) { + return setConstantRaw(name, value); + } + + public boolean setConstant(String name, Integer value) { + return setConstantRaw(name, value); + } + + public boolean setConstant(String name, String value) { + return setConstantRaw(name, value); + } + + private boolean setConstantRaw(String name, Object value) { + boolean success = false; + Field[] declaredFields = this.getClass().getDeclaredFields(); + for (Field field : declaredFields) { + if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { + try { + Object current = field.get(this); + field.set(this, value); + success = true; + if (!value.equals(current)) { + modifiedKeys.put(name, true); + } + } catch (IllegalArgumentException | IllegalAccessException e) { + System.out.println("Could not set field: " + name); + } + } + } + return success; + } + + public Object getValueForConstant(String name) throws Exception { + Field[] declaredFields = this.getClass().getDeclaredFields(); + for (Field field : declaredFields) { + if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { + try { + return field.get(this); + } catch (IllegalArgumentException | IllegalAccessException e) { + throw new Exception("Constant not found"); + } + } + } + throw new Exception("Constant not found"); + } + + public Constant getConstant(String name) { + Field[] declaredFields = this.getClass().getDeclaredFields(); + for (Field field : declaredFields) { + if (java.lang.reflect.Modifier.isStatic(field.getModifiers()) && field.getName().equals(name)) { + try { + return new Constant(field.getName(), field.getType(), field.get(this)); + } catch (IllegalArgumentException | IllegalAccessException e) { + e.printStackTrace(); + } + } + } + return new Constant("", Object.class, 0); + } + + public Collection getConstants() { + List constants = (List) getAllConstants(); + int stop = constants.size() - 1; + for (int i = 0; i < constants.size(); ++i) { + Constant c = constants.get(i); + if ("kEndEditableArea".equals(c.name)) { + stop = i; + } + } + return constants.subList(0, stop); + } + + private Collection getAllConstants() { + Field[] declaredFields = this.getClass().getDeclaredFields(); + List constants = new ArrayList(declaredFields.length); + for (Field field : declaredFields) { + if (java.lang.reflect.Modifier.isStatic(field.getModifiers())) { + Constant c; + try { + c = new Constant(field.getName(), field.getType(), field.get(this)); + constants.add(c); + } catch (IllegalArgumentException | IllegalAccessException e) { + e.printStackTrace(); + } + } + } + return constants; + } + + public JSONObject getJSONObjectFromFile() throws IOException, ParseException { + File file = getFile(); + if (file == null || !file.exists()) { + return new JSONObject(); + } + FileReader reader; + reader = new FileReader(file); + JSONParser jsonParser = new JSONParser(); + return (JSONObject) jsonParser.parse(reader); + } + + public void loadFromFile() { + try { + JSONObject jsonObject = getJSONObjectFromFile(); + Set keys = jsonObject.keySet(); + for (Object o : keys) { + String key = (String) o; + Object value = jsonObject.get(o); + if (value instanceof Long && getConstant(key).type.equals(int.class)) { + value = new BigDecimal((Long) value).intValueExact(); + } + setConstantRaw(key, value); + } + } catch (IOException e) { + e.printStackTrace(); + } catch (ParseException e) { + e.printStackTrace(); + } + } + + public void saveToFile() { + File file = getFile(); + if (file == null) { + return; + } + try { + JSONObject json = getJSONObjectFromFile(); + FileWriter writer = new FileWriter(file); + for (String key : modifiedKeys.keySet()) { + try { + Object value = getValueForConstant(key); + json.put(key, value); + } catch (Exception e) { + e.printStackTrace(); + } + } + writer.write(json.toJSONString()); + writer.close(); + } catch (IOException | ParseException e) { + e.printStackTrace(); + } + } + +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java new file mode 100644 index 0000000..c53b589 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLoopable.java @@ -0,0 +1,7 @@ +package org.usfirst.frc4388.utility; + +public interface ControlLoopable +{ + public void controlLoopUpdate(); + public void setPeriodMs(long periodMs); +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java new file mode 100644 index 0000000..ac7be21 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/ControlLooper.java @@ -0,0 +1,79 @@ +package org.usfirst.frc4388.utility; + +import java.util.Timer; +import java.util.TimerTask; +import java.util.Vector; + +/** + * ControlLooper.java + *

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