diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..61f25ce --- /dev/null +++ b/.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/README.md b/README.md new file mode 100644 index 0000000..6525af3 --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# lib6647 +Team 6647's library to allow JSON-oriented object building and initialization. diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..006a0c0 --- /dev/null +++ b/build.gradle @@ -0,0 +1,55 @@ +/* + * This file was generated by the Gradle 'init' task. + * + * This generated file contains a sample Java Library project to get you started. + * For more details take a look at the Java Libraries chapter in the Gradle + * User Manual available at https://docs.gradle.org/5.4.1/userguide/java_library_plugin.html + */ + +plugins { + // Apply the java-library plugin to add support for Java Library + id 'java' + id 'java-library' + id "edu.wpi.first.GradleRIO" version "2019.4.1" +} + +repositories { + // Use jcenter for resolving your dependencies. + // You can declare any Maven/Ivy/file repository here. + jcenter() + mavenCentral() + maven { + url "http://devsite.ctr-electronics.com/maven/release" + } +} + +dependencies { + + //WPILib 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) + + // This dependency is exported to consumers, that is to say found on their compile classpath. + api 'org.apache.commons:commons-math3:3.6.1' + + // This dependency is used internally, and not exposed to consumers on their own compile classpath. + implementation 'com.google.guava:guava:27.0.1-jre' + + // Use JUnit test framework + testImplementation 'junit:junit:4.12' + + // https://mvnrepository.com/artifact/com.googlecode.json-simple/json-simple + compile group: 'com.googlecode.json-simple', name: 'json-simple', version: '1.1.1' + + //Phoenix dependencies. + compile group: 'com.ctre.phoenix', name: 'api-java', version: '5.14.1' + compile group: 'com.ctre.phoenix', name: 'wpiapi-java', version: '5.14.1' +} + +// 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. +jar { + from { configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } } +} diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..5c2d1cf Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..ab67bfa --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=permwrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-5.4.1-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=permwrapper/dists diff --git a/gradlew b/gradlew new file mode 100644 index 0000000..b0d6d0a --- /dev/null +++ b/gradlew @@ -0,0 +1,188 @@ +#!/usr/bin/env sh + +# +# Copyright 2015 the original author or authors. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >/dev/null +APP_HOME="`pwd -P`" +cd "$SAVED" >/dev/null + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"' + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn () { + echo "$*" +} + +die () { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; + NONSTOP* ) + nonstop=true + ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin, 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/gradlew.bat b/gradlew.bat new file mode 100644 index 0000000..9991c50 --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,100 @@ +@rem +@rem Copyright 2015 the original author or authors. +@rem +@rem Licensed under the Apache License, Version 2.0 (the "License"); +@rem you may not use this file except in compliance with the License. +@rem You may obtain a copy of the License at +@rem +@rem http://www.apache.org/licenses/LICENSE-2.0 +@rem +@rem Unless required by applicable law or agreed to in writing, software +@rem distributed under the License is distributed on an "AS IS" BASIS, +@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +@rem See the License for the specific language governing permissions and +@rem limitations under the License. +@rem + +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m" + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windows variants + +if not "%OS%" == "Windows_NT" goto win9xME_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/settings.gradle b/settings.gradle new file mode 100644 index 0000000..b0f4d48 --- /dev/null +++ b/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/src/main/java/org/usfirst/lib6647/oi/ButtonHelper.java b/src/main/java/org/usfirst/lib6647/oi/ButtonHelper.java new file mode 100644 index 0000000..421bf7b --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/oi/ButtonHelper.java @@ -0,0 +1,107 @@ +package org.usfirst.lib6647.oi; + +import java.io.FileReader; +import java.io.IOException; +import java.io.Reader; +import java.util.HashMap; + +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; +import org.json.simple.parser.ParseException; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.buttons.Button; + +/** + * Helper class for registering button input. + */ +public class ButtonHelper { + public HashMap joysticks; + String fileName; + + /** + * Constructor for the class. + * + * @param fileName + */ + public ButtonHelper(String fileName) { + this.fileName = fileName; + + joysticks = new HashMap(); + } + + /** + * Method for getting a button with a JSON name from a given joystick. Returns + * null if no button is found. + * + * @param joystick + * @param buttonName + * @return button from the given joystick + */ + public Button oiButton(String joystickName, String buttonName) { + try { + JSONParser parser = new JSONParser(); + Reader file = new FileReader(fileName); + JSONObject jsonJoystick = (JSONObject) ((JSONObject) parser.parse(file)) + .get(joysticks.get(joystickName).getName()); + + Button button = joysticks.get(joystickName).buttons.get(jsonJoystick.get(buttonName).toString()); + + jsonJoystick.clear(); + file.close(); + parser.reset(); + + return button; + } catch (IOException e) { + DriverStation.reportError("[!] OIBUTTON " + buttonName + " IO ERROR: " + e.getMessage(), false); + System.out.println("[!] OIBUTTON " + buttonName + " IO ERROR: " + e.getMessage()); + System.exit(1); + } catch (ParseException e) { + DriverStation.reportError("[!] OIBUTTON " + buttonName + " PARSE ERROR: " + e.getMessage(), false); + System.out.println("[!] OIBUTTON " + buttonName + " PARSE ERROR: " + e.getMessage()); + System.exit(1); + } catch (Exception e) { + DriverStation.reportError("[!] OIBUTTON " + buttonName + " ERROR: " + e.getMessage(), false); + System.out.println("[!] OIBUTTON " + buttonName + " ERROR: " + e.getMessage()); + System.exit(1); + } + return null; + } + + /** + * Method for getting a button from a given joystick. + * + * @param joystickName + * @param button + * @return button from the given joystick + */ + public Button oiButton(String joystickName, int button) { + return joysticks.get(joystickName).buttons.get("Button" + button); + } + + /** + * Method for getting an axisButton from a given joystick. + * + * @param joystickName + * @param type + * @param axis + * @return axisButton from the given joystick, for the given axis + */ + public Button oiButton(String joystickName, String type, int axis) { + return joysticks.get(joystickName).buttons.get(type + axis); + } + + /** + * Method for getting an axisButton from a given joystick, at a specific angle. + * + * @param joystickName + * @param type + * @param axis + * @param angle + * @return axisButton from the given joystick, for the given axis, for the given + * angle + */ + public Button oiButton(String joystickName, String type, int axis, int angle) { + return joysticks.get(joystickName).buttons.get(type + axis + "_" + angle); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/oi/JController.java b/src/main/java/org/usfirst/lib6647/oi/JController.java new file mode 100644 index 0000000..0da4a14 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/oi/JController.java @@ -0,0 +1,139 @@ +package org.usfirst.lib6647.oi; + +import java.util.HashMap; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj.buttons.JoystickButton; + +/** + * Joystick wrapper for initializing buttons. + */ +public class JController extends Joystick { + + public HashMap buttons = new HashMap(); + private int leftAxis = 1, rightAxis = 5; + + /** + * Constructor for the controller. + * + * Initializes each and every button from the joystick found at the given port. + * Also initializes buttons for certain angles for each of the axis buttons. + * + * @param port + */ + public JController(int port) { + super(port); + + for (int i = 1; i <= this.getButtonCount(); i++) { + buttons.put("Button" + i, new JoystickButton(this, i)); + } + + for (int i = 0; i < this.getPOVCount(); i++) { + buttons.put("dPad" + i, buttonFromPOV(this, i)); + for (int j = 0; j <= 315; j += 45) { + buttons.put("dPad" + i + "_" + j, buttonFromPOV(this, i, j)); + } + } + + for (int i = 0; i < this.getAxisCount(); i++) { + buttons.put("Stick" + i + "_1", buttonFromAxisPositive(this, i)); + buttons.put("Stick" + i + "_-1", buttonFromAxisNegative(this, i)); + } + } + + /** + * Method to set left and right axis. + * + * @param leftAxis + * @param rightAxis + */ + public void setLeftRightAxis(int leftAxis, int rightAxis) { + this.leftAxis = leftAxis; + this.rightAxis = rightAxis; + } + + /** + * Method to get left-most Stick raw value. + * + * @return leftAxis + */ + public double getLeftAxis() { + return getRawAxis(leftAxis); + } + + /** + * Method to get right-most Stick raw value. + * + * @return rightAxis + */ + public double getRightAxis() { + return getRawAxis(rightAxis); + } + + /** + * Method for getting a pov button at any angle. + * + * @param controller + * @param pov + * @return axisButton + */ + private Button buttonFromPOV(GenericHID controller, int pov) { + return new Button() { + @Override + public boolean get() { + return controller.getPOV(pov) > -1; + } + }; + } + + /** + * Method for getting a pov button at a specific angle. + * + * @param controller + * @param pov + * @param angle + * @return povButton + */ + private Button buttonFromPOV(GenericHID controller, int pov, int angle) { + return new Button() { + @Override + public boolean get() { + return controller.getPOV(pov) == angle; + } + }; + } + + /** + * Method for getting a negative input from an axis. + * + * @param controller + * @param axis + * @return axisButton + */ + private Button buttonFromAxisNegative(GenericHID controller, int axis) { + return new Button() { + @Override + public boolean get() { + return controller.getRawAxis(axis) < -0.15; + } + }; + } + + /** + * Method for getting a positive input from an axis. + * + * @param controller + * @param axis + * @return axisButton + */ + private Button buttonFromAxisPositive(GenericHID controller, int axis) { + return new Button() { + @Override + public boolean get() { + return controller.getRawAxis(axis) > 0.15; + } + }; + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/PIDSuperSubsystem.java b/src/main/java/org/usfirst/lib6647/subsystem/PIDSuperSubsystem.java new file mode 100644 index 0000000..e39dd98 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/PIDSuperSubsystem.java @@ -0,0 +1,114 @@ +package org.usfirst.lib6647.subsystem; + +import java.io.FileReader; +import java.io.Reader; + +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.command.PIDSubsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Class to allow usage of JSON files for PIDSubsystem creation. + */ +public abstract class PIDSuperSubsystem extends PIDSubsystem { + + protected JSONObject robotMap; + protected float p = 0.0f, i = 0.0f, d = 0.0f; + + /** + * Constructor for the class. + * + * @param name + * @param fileName (of JSON) + */ + public PIDSuperSubsystem(String name, String fileName) { + super(name, 0.0f, 0.0f, 0.0f); + + initJSON(fileName); + initPID(); + outputPIDValues(getName(), p, i, d); + } + + /** + * Method to initialize JSON at the given path. + * + * @param fileName + */ + private void initJSON(String fileName) { + try { + JSONParser parser = new JSONParser(); + Reader file = new FileReader(fileName); + robotMap = (JSONObject) parser.parse(file); + file.close(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + getName().toUpperCase() + "' JSON INIT ERROR: " + e.getMessage(), false); + System.out.println("[!] SUBSYSTEM '" + getName().toUpperCase() + "' JSON INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } + + /** + * Method to initialize subsystem PID values and configuration. + */ + private void initPID() { + try { + JSONObject pid = (JSONObject) ((JSONObject) ((JSONObject) robotMap.get("subsystems")).get(getName())) + .get("pid"); + + p = Float.parseFloat(pid.get("p").toString()); + i = Float.parseFloat(pid.get("i").toString()); + d = Float.parseFloat(pid.get("d").toString()); + + getPIDController().setPID(p, i, d); + setInputRange(Double.parseDouble(pid.get("inputMin").toString()), + Double.parseDouble(pid.get("inputMax").toString())); + setOutputRange(Double.parseDouble(pid.get("outputMin").toString()), + Double.parseDouble(pid.get("outputMax").toString())); + setAbsoluteTolerance(Double.parseDouble(pid.get("absoluteTolerance").toString())); + getPIDController().setContinuous(Boolean.parseBoolean(pid.get("continuous").toString())); + + pid.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + getName().toUpperCase() + "' PID INIT ERROR: " + e.getMessage(), false); + System.out.println("[!] SUBSYSTEM '" + getName().toUpperCase() + "' PID INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } + + /** + * Method to clear JSONObject. + */ + public void finishedJSONInit() { + robotMap.clear(); + } + + /** + * Method to output PID values to the SmartDashboard. + * + * @param subsystemName + * @param p + * @param i + * @param d + */ + private void outputPIDValues(String subsystemName, float p, float i, float d) { + SmartDashboard.putString(subsystemName + "P", p + ""); + SmartDashboard.putString(subsystemName + "I", i + ""); + SmartDashboard.putString(subsystemName + "D", d + ""); + } + + /** + * Method to update PID values from the SmartDashboard. + */ + public void updatePIDValues() { + p = Float.parseFloat(SmartDashboard.getString(getName() + "P", p + "")); + i = Float.parseFloat(SmartDashboard.getString(getName() + "I", i + "")); + d = Float.parseFloat(SmartDashboard.getString(getName() + "D", d + "")); + + getPIDController().setPID(p, i, d); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/SuperSubsystem.java b/src/main/java/org/usfirst/lib6647/subsystem/SuperSubsystem.java new file mode 100644 index 0000000..ee90f3a --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/SuperSubsystem.java @@ -0,0 +1,56 @@ +package org.usfirst.lib6647.subsystem; + +import java.io.FileReader; +import java.io.Reader; + +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.command.Subsystem; + +/** + * Class to allow usage of JSON files for Subsystem creation. + */ +public abstract class SuperSubsystem extends Subsystem { + + protected JSONObject robotMap; + + /** + * Constructor for the class. + * + * @param name + * @param fileName (of JSON) + */ + public SuperSubsystem(String name, String fileName) { + super(name); + + initJSON(fileName); + } + + /** + * Method to initialize JSON at the given path. + * + * @param fileName + */ + private void initJSON(String fileName) { + try { + JSONParser parser = new JSONParser(); + Reader file = new FileReader(fileName); + robotMap = (JSONObject) parser.parse(file); + file.close(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + getName().toUpperCase() + "' JSON INIT ERROR: " + e.getMessage(), false); + System.out.println("[!] SUBSYSTEM '" + getName().toUpperCase() + "' JSON INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } + + /** + * Method to clear JSONObject. + */ + public void finishedJSONInit() { + robotMap.clear(); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/components/SuperCompressor.java b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperCompressor.java new file mode 100644 index 0000000..2a473e7 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperCompressor.java @@ -0,0 +1,57 @@ +package org.usfirst.lib6647.subsystem.components; + +import java.util.Arrays; +import java.util.HashMap; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; + +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.DriverStation; + +/** + * Interface to allow Compressor initialization via JSON file. Subsystem needs + * to extend SuperSubsystem. + */ +public interface SuperCompressor { + /** + * HashMap storing the SuperSubsystem's Compressors. + */ + public HashMap compressors = new HashMap(); + + /** + * Method to initialize Compressors declared in JSON file, and add them to the + * HashMap using its name as its key. + * + * @param robotMap + * @param subsystemName + */ + default void initCompressors(JSONObject robotMap, String subsystemName) { + try { + JSONArray compressorArray = (JSONArray) ((JSONObject) ((JSONObject) robotMap.get("subsystems")) + .get(subsystemName)).get("compressors"); + Arrays.stream(compressorArray.toArray()).map(json -> (JSONObject) json).forEach(json -> { + try { + Compressor compressor = new Compressor(Integer.parseInt(json.get("module").toString())); + compressors.put(json.get("name").toString(), compressor); + } catch (Exception e) { + DriverStation.reportError("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + + "' COMPRESSOR INIT ERROR: " + e.getMessage(), false); + System.out.println("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' COMPRESSOR INIT ERROR: " + + e.getMessage()); + System.exit(1); + } finally { + json.clear(); + } + }); + compressorArray.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' COMPRESSOR INIT ERROR: " + e.getMessage(), + false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' COMPRESSOR INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/components/SuperDigitalInput.java b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperDigitalInput.java new file mode 100644 index 0000000..f7906e7 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperDigitalInput.java @@ -0,0 +1,57 @@ +package org.usfirst.lib6647.subsystem.components; + +import java.util.Arrays; +import java.util.HashMap; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; + +/** + * Interface to allow DigitalInput initialization via JSON file. Subsystem needs + * to extend SuperSubsystem. + */ +public interface SuperDigitalInput { + /** + * HashMap storing the SuperSubsystem's DigitalInputs. + */ + public HashMap digitalInputs = new HashMap(); + + /** + * Method to initialize DigitalInputs declared in JSON file, and add them to the + * HashMap using its name as its key. + * + * @param robotMap + * @param subsystemName + */ + default void initDigitalInputs(JSONObject robotMap, String subsystemName) { + try { + JSONArray digitalInputArray = (JSONArray) ((JSONObject) ((JSONObject) robotMap.get("subsystems")) + .get(subsystemName)).get("digitalInputs"); + Arrays.stream(digitalInputArray.toArray()).map(json -> (JSONObject) json).forEach(json -> { + try { + DigitalInput digitalInput = new DigitalInput(Integer.parseInt(json.get("channel").toString())); + digitalInputs.put(json.get("name").toString(), digitalInput); + } catch (Exception e) { + DriverStation.reportError("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + + "' DIGITAL INPUT INIT ERROR: " + e.getMessage(), false); + System.out.println("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' DIGITAL INPUT INIT ERROR: " + + e.getMessage()); + System.exit(1); + } finally { + json.clear(); + } + }); + digitalInputArray.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' DIGITAL INPUT INIT ERROR: " + e.getMessage(), + false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' DIGITAL INPUT INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/components/SuperEncoder.java b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperEncoder.java new file mode 100644 index 0000000..58224aa --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperEncoder.java @@ -0,0 +1,76 @@ +package org.usfirst.lib6647.subsystem.components; + +import java.util.Arrays; +import java.util.HashMap; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; + +import edu.wpi.first.wpilibj.CounterBase.EncodingType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Encoder; + +/** + * Interface to allow Encoder initialization via JSON file. Subsystem needs to + * extend SuperSubsystem. + */ +public interface SuperEncoder { + /** + * HashMap storing the SuperSubsystem's Encoders. + */ + public HashMap encoders = new HashMap(); + + /** + * Method to initialize Encoders declared in JSON file, and add them to the + * HashMap using its name as its key. + * + * @param robotMap + * @param subsystemName + */ + default void initEncoders(JSONObject robotMap, String subsystemName) { + try { + JSONArray encoderArray = (JSONArray) ((JSONObject) ((JSONObject) robotMap.get("subsystems")) + .get(subsystemName)).get("encoders"); + Arrays.stream(encoderArray.toArray()).map(json -> (JSONObject) json).forEach(json -> { + try { + Encoder encoder = new Encoder(Integer.parseInt(json.get("channelA").toString()), + Integer.parseInt(json.get("channelB").toString()), + Boolean.parseBoolean(json.get("reverse").toString()), + getEncodingType(json.get("encodingType").toString())); + encoder.reset(); + + encoders.put(json.get("name").toString(), encoder); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' ENCODER INIT ERROR: " + e.getMessage(), + false); + System.out.println("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' ENCODER INIT ERROR: " + + e.getMessage()); + System.exit(1); + } finally { + json.clear(); + } + }); + encoderArray.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' ENCODER INIT ERROR: " + e.getMessage(), false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' ENCODER INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } + + private EncodingType getEncodingType(String encodingType) { + switch (encodingType) { + case "k1X": + return EncodingType.k1X; + case "k2X": + return EncodingType.k2X; + case "k4X": + return EncodingType.k4X; + default: + return null; + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/components/SuperPDP.java b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperPDP.java new file mode 100644 index 0000000..5f50f91 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperPDP.java @@ -0,0 +1,57 @@ +package org.usfirst.lib6647.subsystem.components; + +import java.util.Arrays; +import java.util.HashMap; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PowerDistributionPanel; + +/** + * Interface to allow PowerDistributionPanel initialization via JSON file. + * Subsystem needs to extend SuperSubsystem. + */ +public interface SuperPDP { + /** + * HashMap storing the SuperSubsystem's PowerDistributionPanel objects. + */ + public HashMap PDPs = new HashMap(); + + /** + * Method to initialize PDPs declared in JSON file, and add them to the HashMap + * using its name as its key. + * + * @param robotMap + * @param subsystemName + */ + default void initPDPs(JSONObject robotMap, String subsystemName) { + try { + JSONArray PDPArray = (JSONArray) ((JSONObject) ((JSONObject) robotMap.get("subsystems")).get(subsystemName)) + .get("PDPs"); + Arrays.stream(PDPArray.toArray()).map(json -> (JSONObject) json).forEach(json -> { + try { + PowerDistributionPanel pdp = new PowerDistributionPanel( + Integer.parseInt(json.get("module").toString())); + PDPs.put(json.get("name").toString(), pdp); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' PDP INIT ERROR: " + e.getMessage(), + false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' PDP INIT ERROR: " + e.getMessage()); + System.exit(1); + } finally { + json.clear(); + } + }); + PDPArray.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' PDP INIT ERROR: " + e.getMessage(), false); + System.out.println("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' PDP INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/components/SuperSolenoid.java b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperSolenoid.java new file mode 100644 index 0000000..8a2c637 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperSolenoid.java @@ -0,0 +1,61 @@ +package org.usfirst.lib6647.subsystem.components; + +import java.util.Arrays; +import java.util.HashMap; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Solenoid; + +/** + * Interface to allow Solenoid initialization via JSON file. Subsystem needs to + * extend SuperSubsystem. + */ +public interface SuperSolenoid { + /** + * HashMap storing the SuperSubsystem's Solenoids. + */ + public HashMap solenoids = new HashMap(); + + /** + * Method to initialize Solenoids declared in JSON file, and add them to the + * HashMap using its name as its key. + * + * @param robotMap + * @param subsystemName + */ + default void initSolenoids(JSONObject robotMap, String subsystemName) { + try { + JSONArray solenoidArray = (JSONArray) ((JSONObject) ((JSONObject) robotMap.get("subsystems")) + .get(subsystemName)).get("solenoids"); + Arrays.stream(solenoidArray.toArray()).map(json -> (JSONObject) json).forEach(json -> { + try { + Solenoid solenoid = new Solenoid(Integer.parseInt(json.get("channel").toString())); + + if (json.containsKey("initialValue")) + solenoid.set(Boolean.parseBoolean(json.get("initialValue").toString())); + + solenoids.put(json.get("name").toString(), solenoid); + } catch (Exception e) { + DriverStation.reportError("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + + "' SOLENOID INIT ERROR: " + e.getMessage(), false); + System.out.println("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' SOLENOID INIT ERROR: " + + e.getMessage()); + System.exit(1); + } finally { + json.clear(); + } + }); + solenoidArray.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' SOLENOID INIT ERROR: " + e.getMessage(), + false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' SOLENOID INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/components/SuperTalon.java b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperTalon.java new file mode 100644 index 0000000..ca37d64 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperTalon.java @@ -0,0 +1,76 @@ +package org.usfirst.lib6647.subsystem.components; + +import java.util.Arrays; +import java.util.HashMap; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; +import org.usfirst.lib6647.util.MotorUtils; + +import edu.wpi.first.wpilibj.DriverStation; + +/** + * Interface to allow Talon initialization via JSON file. Subsystem needs to + * extend SuperSubsystem. + */ +public interface SuperTalon extends MotorUtils { + /** + * HashMap storing the SuperSubsystem's Talons. + */ + public HashMap talons = new HashMap(); + + /** + * Method to initialize Talons declared in JSON file, and add them to the + * HashMap using its name as its key. ALWAYS declare and initialize masters + * before followers in the JSON file. + * + * @param robotMap + * @param subsystemName + */ + default void initTalons(JSONObject robotMap, String subsystemName) { + try { + JSONArray talonArray = (JSONArray) ((JSONObject) ((JSONObject) robotMap.get("subsystems")) + .get(subsystemName)).get("talons"); + Arrays.stream(talonArray.toArray()).map(json -> (JSONObject) json).forEach(json -> { + try { + WPI_TalonSRX talon = new WPI_TalonSRX(Integer.parseInt(json.get("port").toString())); + + if (json.containsKey("inverted")) + setInverted(json, talon); + + if (json.containsKey("neutralMode")) + setNeutralMode(json, talon); + + if (json.containsKey("loopRamp")) + setLoopRamp(json, talon); + + if (json.containsKey("sensor")) + setSensors(json, talon); + + if (json.containsKey("pid")) + setPIDValues(json, talon); + + talons.put(json.get("name").toString(), talon); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' TALON INIT ERROR: " + e.getMessage(), + false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' TALON INIT ERROR: " + e.getMessage()); + System.exit(1); + } finally { + json.clear(); + } + }); + talonArray.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' TALON INIT ERROR: " + e.getMessage(), false); + System.out + .println("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' TALON INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/components/SuperUltrasonic.java b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperUltrasonic.java new file mode 100644 index 0000000..a80ed62 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperUltrasonic.java @@ -0,0 +1,58 @@ +package org.usfirst.lib6647.subsystem.components; + +import java.util.Arrays; +import java.util.HashMap; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Ultrasonic; + +/** + * Interface to allow Ultrasonic initialization via JSON file. Subsystem needs + * to extend SuperSubsystem. + */ +public interface SuperUltrasonic { + /** + * HashMap storing the SuperSubsystem's Ultrasonics. + */ + public HashMap ultrasonics = new HashMap(); + + /** + * Method to initialize Ultrasonics declared in JSON file, and add them to the + * HashMap using its name as its key. + * + * @param robotMap + * @param subsystemName + */ + default void initUltrasonics(JSONObject robotMap, String subsystemName) { + try { + JSONArray ultrasonicArray = (JSONArray) ((JSONObject) ((JSONObject) robotMap.get("subsystems")) + .get(subsystemName)).get("ultrasonics"); + Arrays.stream(ultrasonicArray.toArray()).map(json -> (JSONObject) json).forEach(json -> { + try { + Ultrasonic ultrasonic = new Ultrasonic(Integer.parseInt(json.get("pingChannel").toString()), + Integer.parseInt(json.get("echoChannel").toString())); + ultrasonics.put(json.get("name").toString(), ultrasonic); + } catch (Exception e) { + DriverStation.reportError("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + + "' ULTRASONIC INIT ERROR: " + e.getMessage(), false); + System.out.println("[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' ULTRASONIC INIT ERROR: " + + e.getMessage()); + System.exit(1); + } finally { + json.clear(); + } + }); + ultrasonicArray.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' ULTRASONIC INIT ERROR: " + e.getMessage(), + false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' ULTRASONIC INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/subsystem/components/SuperVictor.java b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperVictor.java new file mode 100644 index 0000000..05a5d0c --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/subsystem/components/SuperVictor.java @@ -0,0 +1,70 @@ +package org.usfirst.lib6647.subsystem.components; + +import java.util.Arrays; +import java.util.HashMap; + +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; + +import org.json.simple.JSONArray; +import org.json.simple.JSONObject; +import org.usfirst.lib6647.util.MotorUtils; + +import edu.wpi.first.wpilibj.DriverStation; + +/** + * Interface to allow Victor initialization via JSON file. Subsystem needs to + * extend SuperSubsystem. + */ +public interface SuperVictor extends MotorUtils { + /** + * HashMap storing the SuperSubsystem's Victors. + */ + public HashMap victors = new HashMap(); + + /** + * Method to initialize Victors declared in JSON file, and add them to the + * HashMap using its name as its key. ALWAYS declare and initialize masters + * before followers! + * + * @param robotMap + * @param subsystemName + */ + default void initVictors(JSONObject robotMap, String subsystemName) { + try { + JSONArray victorArray = (JSONArray) ((JSONObject) ((JSONObject) robotMap.get("subsystems")) + .get(subsystemName)).get("victors"); + Arrays.stream(victorArray.toArray()).map(json -> (JSONObject) json).forEach(json -> { + try { + WPI_VictorSPX victor = new WPI_VictorSPX(Integer.parseInt(json.get("port").toString())); + + if (json.containsKey("inverted")) + setInverted(json, victor); + + if (json.containsKey("neutralMode")) + setNeutralMode(json, victor); + + if (json.containsKey("loopRamp")) + setLoopRamp(json, victor); + + victors.put(json.get("name").toString(), victor); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' VICTOR INIT ERROR: " + e.getMessage(), + false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' VICTOR INIT ERROR: " + e.getMessage()); + System.exit(1); + } finally { + json.clear(); + } + }); + victorArray.clear(); + } catch (Exception e) { + DriverStation.reportError( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' VICTOR INIT ERROR: " + e.getMessage(), false); + System.out.println( + "[!] SUBSYSTEM '" + subsystemName.toUpperCase() + "' VICTOR INIT ERROR: " + e.getMessage()); + System.exit(1); + } + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/util/Direction.java b/src/main/java/org/usfirst/lib6647/util/Direction.java new file mode 100644 index 0000000..94edb31 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/util/Direction.java @@ -0,0 +1,8 @@ +package org.usfirst.lib6647.util; + +/** + * Enum listing possible movement directions, for readability purposes. + */ +public enum Direction { + UP, DOWN, FRONT, BACK, LEFT, RIGHT, FORWARD, BACKWARD, IN, OUT, NONE +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/lib6647/util/MotorUtils.java b/src/main/java/org/usfirst/lib6647/util/MotorUtils.java new file mode 100644 index 0000000..9761b51 --- /dev/null +++ b/src/main/java/org/usfirst/lib6647/util/MotorUtils.java @@ -0,0 +1,195 @@ +package org.usfirst.lib6647.util; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; + +import org.json.simple.JSONObject; + +/** + * Interface for converting String values to CTRE Objects. + */ +public interface MotorUtils { + + /** + * Sets a given Talon's inverted status from the JSON configuration. + * + * @param json + * @param talon + * @throws Exception + */ + default void setInverted(JSONObject json, WPI_TalonSRX talon) throws Exception { + talon.setInverted(Boolean.parseBoolean(json.get("inverted").toString())); + } + + /** + * Sets a given Victor's inverted status from the JSON configuration. + * + * @param json + * @param victor + * @throws Exception + */ + default void setInverted(JSONObject json, WPI_VictorSPX victor) throws Exception { + victor.setInverted(Boolean.parseBoolean(json.get("inverted").toString())); + } + + /** + * Method to convert a String to its respective NeutralMode. + * + * @param neutralMode + * @return NeutralMode + */ + default NeutralMode getNeutralMode(String neutralMode) { + switch (neutralMode) { + case "Coast": + return NeutralMode.Coast; + case "Brake": + return NeutralMode.Brake; + case "EEPROMSetting": + return NeutralMode.EEPROMSetting; + default: + return null; + } + } + + /** + * Sets a given Talon's inverted status from the JSON configuration. + * + * @param json + * @param talon + * @throws Exception + */ + default void setNeutralMode(JSONObject json, WPI_TalonSRX talon) throws Exception { + talon.setNeutralMode(getNeutralMode(json.get("neutralMode").toString())); + } + + /** + * Sets a given Victor's inverted status from the JSON configuration. + * + * @param json + * @param victor + * @throws Exception + */ + default void setNeutralMode(JSONObject json, WPI_VictorSPX victor) throws Exception { + victor.setNeutralMode(getNeutralMode(json.get("neutralMode").toString())); + } + + /** + * Sets a given Talon's loopramp from the JSON configuration. + * + * @param json + * @param talon + * @throws Exception + */ + default void setLoopRamp(JSONObject json, WPI_TalonSRX talon) throws Exception { + JSONObject closed = (JSONObject) ((JSONObject) json.get("loopRamp")).get("closed"), + open = (JSONObject) ((JSONObject) json.get("loopRamp")).get("open"); + + if (closed.containsKey("timeoutMs") && open.containsKey("timeoutMs")) { + talon.configClosedloopRamp(Double.parseDouble(closed.get("secondsFromNeutralToFull").toString()), + Integer.parseInt(closed.get("timeoutMs").toString())); + talon.configOpenloopRamp(Double.parseDouble(open.get("secondsFromNeutralToFull").toString()), + Integer.parseInt(open.get("timeoutMs").toString())); + } else { + talon.configClosedloopRamp(Double.parseDouble(closed.get("secondsFromNeutralToFull").toString())); + talon.configOpenloopRamp(Double.parseDouble(open.get("secondsFromNeutralToFull").toString())); + } + } + + /** + * Sets a given Victor's loopramp from the JSON configuration. + * + * @param json + * @param victor + * @throws Exception + */ + default void setLoopRamp(JSONObject json, WPI_VictorSPX victor) throws Exception { + JSONObject closed = (JSONObject) ((JSONObject) json.get("loopRamp")).get("closed"), + open = (JSONObject) ((JSONObject) json.get("loopRamp")).get("open"); + + if (closed.containsKey("timeoutMs") && open.containsKey("timeoutMs")) { + victor.configClosedloopRamp(Double.parseDouble(closed.get("secondsFromNeutralToFull").toString()), + Integer.parseInt(closed.get("timeoutMs").toString())); + victor.configOpenloopRamp(Double.parseDouble(open.get("secondsFromNeutralToFull").toString()), + Integer.parseInt(open.get("timeoutMs").toString())); + } else { + victor.configClosedloopRamp(Double.parseDouble(closed.get("secondsFromNeutralToFull").toString())); + victor.configOpenloopRamp(Double.parseDouble(open.get("secondsFromNeutralToFull").toString())); + } + } + + /** + * Method to convert a String to its respective FeedbackDevice. + * + * @param feedbackDevice + * @return FeedbackDevice + */ + default FeedbackDevice getFeedbackDevice(String feedbackDevice) { + switch (feedbackDevice) { + case "QuadEncoder": + return FeedbackDevice.QuadEncoder; + case "Analog": + return FeedbackDevice.Analog; + case "Tachometer": + return FeedbackDevice.Tachometer; + case "PulseWidthEncodedPosition": + return FeedbackDevice.PulseWidthEncodedPosition; + case "SensorSum": + return FeedbackDevice.SensorSum; + case "SensorDifference": + return FeedbackDevice.SensorDifference; + case "RemoteSensor0": + return FeedbackDevice.RemoteSensor0; + case "RemoteSensor1": + return FeedbackDevice.RemoteSensor1; + case "SoftwareEmulatedSensor": + return FeedbackDevice.SoftwareEmulatedSensor; + case "CTRE_MagEncoder_Absolute": + return FeedbackDevice.CTRE_MagEncoder_Absolute; + case "CTRE_MagEncoder_Relative": + return FeedbackDevice.CTRE_MagEncoder_Relative; + default: + return null; + } + } + + /** + * Sets a given Talon's sensors from the JSON configuration (fairly limited in + * terms of customizability at the moment). + * + * @param json + * @param talon + * @throws Exception + */ + default void setSensors(JSONObject json, WPI_TalonSRX talon) throws Exception { + JSONObject sensor = (JSONObject) json.get("sensor"); + JSONObject feedback = (JSONObject) sensor.get("feedback"); + + talon.configSelectedFeedbackSensor(getFeedbackDevice(feedback.get("feedbackDevice").toString()), + Integer.parseInt(feedback.get("pidIdx").toString()), + Integer.parseInt(feedback.get("timeoutMs").toString())); + + talon.setSensorPhase(Boolean.parseBoolean(sensor.get("phase").toString())); + + talon.setSelectedSensorPosition(Integer.parseInt(sensor.get("sensorPos").toString()), + Integer.parseInt(sensor.get("pidIdx").toString()), + Integer.parseInt(sensor.get("timeoutMs").toString())); + } + + /** + * Sets a given Talon's PID values from the JSON configuration. + * + * @param json + * @param talon + * @throws Exception + */ + default void setPIDValues(JSONObject json, WPI_TalonSRX talon) throws Exception { + JSONObject pid = (JSONObject) json.get("pid"); + + talon.config_kP(Integer.parseInt(pid.get("slotIdx").toString()), Double.parseDouble(pid.get("p").toString())); + talon.config_kI(Integer.parseInt(pid.get("slotIdx").toString()), Double.parseDouble(pid.get("i").toString())); + talon.config_kD(Integer.parseInt(pid.get("slotIdx").toString()), Double.parseDouble(pid.get("d").toString())); + talon.config_kF(Integer.parseInt(pid.get("slotIdx").toString()), Double.parseDouble(pid.get("f").toString())); + } +} \ No newline at end of file