diff --git a/C++/MotionMagic/.wpilib/wpilib_preferences.json b/C++/MotionMagic/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..d35f186c --- /dev/null +++ b/C++/MotionMagic/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2019", + "teamNumber": 7762 +} \ No newline at end of file diff --git a/C++/MotionMagic/build.gradle b/C++/MotionMagic/build.gradle new file mode 100644 index 00000000..2972dd43 --- /dev/null +++ b/C++/MotionMagic/build.gradle @@ -0,0 +1,87 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" version "2019.1.1" +} + +// 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 { + frcNativeArtifact('frcCpp') { + targets << "roborio" + component = 'frcUserProgram' + // 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 include the src folder in the include directories passed +// to the compiler. Some eclipse project imports depend on this behavior. +// We recommend leaving this disabled if possible. Note for eclipse project +// imports this is enabled by default. For new projects, its disabled +def includeSrcInIncludeRoot = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +model { + components { + frcUserProgram(NativeExecutableSpec) { + targetPlatform wpi.platforms.roborio + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + if (includeSrcInIncludeRoot) { + srcDir 'src/main/cpp' + } + } + } + + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + useLibrary(it, "wpilib") + wpi.deps.vendor.cpp(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + useLibrary(it, "wpilib", "googletest") + wpi.deps.vendor.cpp(it) + } + } +} diff --git a/C++/MotionMagic/gradle/wrapper/gradle-wrapper.jar b/C++/MotionMagic/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 00000000..457aad0d Binary files /dev/null and b/C++/MotionMagic/gradle/wrapper/gradle-wrapper.jar differ diff --git a/C++/MotionMagic/gradle/wrapper/gradle-wrapper.properties b/C++/MotionMagic/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..d08253ca --- /dev/null +++ b/C++/MotionMagic/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/C++/MotionMagic/gradlew b/C++/MotionMagic/gradlew new file mode 100644 index 00000000..af6708ff --- /dev/null +++ b/C++/MotionMagic/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/C++/MotionMagic/gradlew.bat b/C++/MotionMagic/gradlew.bat new file mode 100644 index 00000000..6d57edc7 --- /dev/null +++ b/C++/MotionMagic/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/C++/MotionMagic/settings.gradle b/C++/MotionMagic/settings.gradle new file mode 100644 index 00000000..b0f4d48a --- /dev/null +++ b/C++/MotionMagic/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/C++/MotionMagic/src/main/cpp/Robot.cpp b/C++/MotionMagic/src/main/cpp/Robot.cpp new file mode 100644 index 00000000..be6c4578 --- /dev/null +++ b/C++/MotionMagic/src/main/cpp/Robot.cpp @@ -0,0 +1,125 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" +#include + +void Robot::RobotInit() { + _talon = new TalonSRX(1); + _joy = new frc::Joystick(0); + + /* Factory default hardware to prevent unexpected behavior */ + _talon->ConfigFactoryDefault(); + + /* Configure Sensor Source for Pirmary PID */ + _talon->ConfigSelectedFeedbackSensor(FeedbackDevice::CTRE_MagEncoder_Relative, + 0, + 10); + + /** + * Configure Talon SRX Output and Sesnor direction accordingly + * Invert Motor to have green LEDs when driving Talon Forward / Requesting Postiive Output + * Phase sensor to have positive increment when driving Talon Forward (Green LED) + */ + _talon->SetSensorPhase(true); + _talon->SetInverted(false); + + /* Set relevant frame periods to be at least as fast as periodic rate */ + _talon->SetStatusFramePeriod(StatusFrameEnhanced::Status_13_Base_PIDF0, 10, 10); + _talon->SetStatusFramePeriod(StatusFrameEnhanced::Status_10_MotionMagic, 10, 10); + + /* Set the peak and nominal outputs */ + _talon->ConfigNominalOutputForward(0, 10); + _talon->ConfigNominalOutputReverse(0, 10); + _talon->ConfigPeakOutputForward(1, 10); + _talon->ConfigPeakOutputReverse(-1, 10); + + /* Set Motion Magic gains in slot0 - see documentation */ + _talon->SelectProfileSlot(0, 0); + _talon->Config_kF(0, 0.3, 10); + _talon->Config_kP(0, 0.1, 10); + _talon->Config_kI(0, 0.0, 10); + _talon->Config_kD(0, 0.0, 10); + + /* Set acceleration and vcruise velocity - see documentation */ + _talon->ConfigMotionCruiseVelocity(1500, 10); + _talon->ConfigMotionAcceleration(1500, 10); + + /* Zero the sensor */ + _talon->SetSelectedSensorPosition(0, 0, 10); +} + +void Robot::AutonomousInit() {} +void Robot::AutonomousPeriodic() {} + +void Robot::TeleopInit() {} +void Robot::TeleopPeriodic() { + /* Get gamepad axis - forward stick is positive */ + double leftYstick = -1.0 * _joy->GetY(); + if (fabs(leftYstick) < 0.10) { leftYstick = 0;} /* deadband 10% */ + + /* Get current Talon SRX motor output */ + double motorOutput = _talon->GetMotorOutputPercent(); + std::stringstream sb; + /* Prepare line to print */ + sb << "\tOut%:" << motorOutput; + sb << "\tVel:" << _talon->GetSelectedSensorVelocity(0); + + if(_joy->GetRawButton(2)) + { + /* Zero the sensor */ + _talon->SetSelectedSensorPosition(0, 0, 10); + } + + /** + * Peform Motion Magic when Button 1 is held, + * else run Percent Output, which can be used to confirm hardware setup. + */ + if (_joy->GetRawButton(1)) { + /* Motion Magic */ + + /*4096 ticks/rev * 10 Rotations in either direction */ + double targetPos = leftYstick * 4096 * 10.0; + _talon->Set(ControlMode::MotionMagic, targetPos); + + /* Append more signals to print when in speed mode */ + sb << "\terr:" << _talon->GetClosedLoopError(0); + sb << "\ttrg:" << targetPos; + } else { + /* Percent Output */ + + _talon->Set(ControlMode::PercentOutput, leftYstick); + } + + if(_joy->GetRawButtonPressed(6)) + { + /* Increase smoothing */ + ++_smoothing; + if(_smoothing > 8) _smoothing = 8; + std::cout << "Smoothing is set to: " << _smoothing << std::endl; + _talon->ConfigMotionSCurveStrength(_smoothing, 0); + } + if(_joy->GetRawButtonPressed(5)) + { + /* Decreasing smoothing */ + --_smoothing; + if(_smoothing < 0) _smoothing = 0; + std::cout << "Smoothing is set to: " << _smoothing << std::endl; + _talon->ConfigMotionSCurveStrength(_smoothing, 0); + } + + + /* Instrumentation */ + Instrum::Process(_talon, &sb); +} + +void Robot::TestInit() {} +void Robot::TestPeriodic() {} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/C++/MotionMagic/src/main/include/Instrum.h b/C++/MotionMagic/src/main/include/Instrum.h new file mode 100644 index 00000000..fece61a6 --- /dev/null +++ b/C++/MotionMagic/src/main/include/Instrum.h @@ -0,0 +1,49 @@ +#pragma once + +#include "frc/smartdashboard/SmartDashboard.h" + +#include "ctre/Phoenix.h" +#include +#include +#include +/** + * Routines for printing to console (FRC Message log). + */ +class Instrum { + /* Tracking variables for instrumentation */ +private: + static int _loops; + static int _timesInMotionMagic; + +public: + static void Process(TalonSRX *tal, std::stringstream *sb) { + /* Smart dash plots */ + frc::SmartDashboard::PutNumber("SensorVel", tal->GetSelectedSensorVelocity(0)); + frc::SmartDashboard::PutNumber("SensorPos", tal->GetSelectedSensorPosition(0)); + frc::SmartDashboard::PutNumber("MotorOutputPercent", tal->GetMotorOutputPercent()); + frc::SmartDashboard::PutNumber("ClosedLoopError", tal->GetClosedLoopError(0)); + + /* Check if Talon SRX is performing Motion Magic */ + if (tal->GetControlMode() == ControlMode::MotionMagic) { + ++_timesInMotionMagic; + } else { + _timesInMotionMagic = 0; + } + + if (_timesInMotionMagic > 10) { + /* Print the Active Trajectory Point Motion Magic is servoing towards */ + frc::SmartDashboard::PutNumber("ClosedLoopTarget", tal->GetClosedLoopTarget(0)); + frc::SmartDashboard::PutNumber("ActTrajVelocity", tal->GetActiveTrajectoryVelocity()); + frc::SmartDashboard::PutNumber("ActTrajPosition", tal->GetActiveTrajectoryPosition()); + } + + /* Periodically print to console */ + if (++_loops >= 20) { + _loops = 0; + std::cout << sb->str() << std::endl; + } + } +}; + +int Instrum::_loops = 0; +int Instrum::_timesInMotionMagic = 0; diff --git a/C++/MotionMagic/src/main/include/Robot.h b/C++/MotionMagic/src/main/include/Robot.h new file mode 100644 index 00000000..7be8c1f3 --- /dev/null +++ b/C++/MotionMagic/src/main/include/Robot.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include "ctre/Phoenix.h" +#include "Instrum.h" + +class Robot : public frc::TimedRobot +{ +public: + void RobotInit() override; + + void AutonomousInit() override; + void AutonomousPeriodic() override; + + void TeleopInit() override; + void TeleopPeriodic() override; + + void TestInit() override; + void TestPeriodic() override; + +private: + TalonSRX *_talon; + frc::Joystick *_joy; + + int _smoothing; +}; diff --git a/C++/MotionMagic/src/test/cpp/main.cpp b/C++/MotionMagic/src/test/cpp/main.cpp new file mode 100644 index 00000000..b8b23d23 --- /dev/null +++ b/C++/MotionMagic/src/test/cpp/main.cpp @@ -0,0 +1,10 @@ +#include + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/C++/MotionMagic/vendordeps/Phoenix.json b/C++/MotionMagic/vendordeps/Phoenix.json new file mode 100644 index 00000000..2f05720b --- /dev/null +++ b/C++/MotionMagic/vendordeps/Phoenix.json @@ -0,0 +1,135 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.13.0", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.13.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.13.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.13.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.13.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.13.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.13.0", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.13.0", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.13.0", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers" + } + ] +} \ No newline at end of file diff --git a/C++/MotionProfileArc_Simple/.wpilib/wpilib_preferences.json b/C++/MotionProfileArc_Simple/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..d35f186c --- /dev/null +++ b/C++/MotionProfileArc_Simple/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2019", + "teamNumber": 7762 +} \ No newline at end of file diff --git a/C++/MotionProfileArc_Simple/build.gradle b/C++/MotionProfileArc_Simple/build.gradle new file mode 100644 index 00000000..2972dd43 --- /dev/null +++ b/C++/MotionProfileArc_Simple/build.gradle @@ -0,0 +1,87 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" version "2019.1.1" +} + +// 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 { + frcNativeArtifact('frcCpp') { + targets << "roborio" + component = 'frcUserProgram' + // 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 include the src folder in the include directories passed +// to the compiler. Some eclipse project imports depend on this behavior. +// We recommend leaving this disabled if possible. Note for eclipse project +// imports this is enabled by default. For new projects, its disabled +def includeSrcInIncludeRoot = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +model { + components { + frcUserProgram(NativeExecutableSpec) { + targetPlatform wpi.platforms.roborio + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + if (includeSrcInIncludeRoot) { + srcDir 'src/main/cpp' + } + } + } + + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + useLibrary(it, "wpilib") + wpi.deps.vendor.cpp(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + useLibrary(it, "wpilib", "googletest") + wpi.deps.vendor.cpp(it) + } + } +} diff --git a/C++/MotionProfileArc_Simple/gradle/wrapper/gradle-wrapper.jar b/C++/MotionProfileArc_Simple/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 00000000..457aad0d Binary files /dev/null and b/C++/MotionProfileArc_Simple/gradle/wrapper/gradle-wrapper.jar differ diff --git a/C++/MotionProfileArc_Simple/gradle/wrapper/gradle-wrapper.properties b/C++/MotionProfileArc_Simple/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..d08253ca --- /dev/null +++ b/C++/MotionProfileArc_Simple/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/C++/MotionProfileArc_Simple/gradlew b/C++/MotionProfileArc_Simple/gradlew new file mode 100644 index 00000000..af6708ff --- /dev/null +++ b/C++/MotionProfileArc_Simple/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/C++/MotionProfileArc_Simple/gradlew.bat b/C++/MotionProfileArc_Simple/gradlew.bat new file mode 100644 index 00000000..6d57edc7 --- /dev/null +++ b/C++/MotionProfileArc_Simple/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/C++/MotionProfileArc_Simple/settings.gradle b/C++/MotionProfileArc_Simple/settings.gradle new file mode 100644 index 00000000..b0f4d48a --- /dev/null +++ b/C++/MotionProfileArc_Simple/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/C++/MotionProfileArc_Simple/src/main/cpp/Robot.cpp b/C++/MotionProfileArc_Simple/src/main/cpp/Robot.cpp new file mode 100644 index 00000000..6bd58bd4 --- /dev/null +++ b/C++/MotionProfileArc_Simple/src/main/cpp/Robot.cpp @@ -0,0 +1,153 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" +#include "MotionProfile.h" +#include "Instrum.h" + +void Robot::RobotInit() +{ + /* Construct global variables */ + _rightMaster = new TalonSRX(1); + _leftMaster = new TalonSRX(2); + _pidgey = new PigeonIMU(3); //This uses a CAN pigeon, as opposed to a gadgeteer pigeon + _joystick = new frc::Joystick(0); + _bufferedStream = new BufferedTrajectoryPointStream(); + + _plotThread = new PlotThread(_rightMaster); + + /* Initialize buffer with motion profile */ + InitBuffer(kMotionProfile, kMotionProfileSz, 0.25); //Do a quarter (0.25) rotation to the left + _state = 0; + + + _masterConfig = new MasterProfileConfiguration(_leftMaster, _pidgey); + _followConfig = new FollowerProfileConfiguration(); + + _rightMaster->ConfigAllSettings(*_masterConfig); + _leftMaster->ConfigAllSettings(*_followConfig); + + _rightMaster->SetSensorPhase(true); + _leftMaster->SetSensorPhase(false); + + _rightMaster->SetInverted(true); + _leftMaster->SetInverted(false); + + _rightMaster->SetStatusFramePeriod(StatusFrameEnhanced::Status_14_Turn_PIDF1, 20); //Telemetry using Phoenix Tuner +} + +void Robot::AutonomousInit() {} +void Robot::AutonomousPeriodic() {} + +void Robot::TeleopInit() {} +void Robot::TeleopPeriodic() +{ + /* get joystick button and stick */ + bool bPrintValues = _joystick->GetRawButton(2); + bool bFireMp = _joystick->GetRawButton(1); + double axis = -_joystick->GetRawAxis(1); + double turn = _joystick->GetRawAxis(2); + + /* if button is up, just drive the motor in PercentOutput */ + if (bFireMp == false) { + _state = 0; + } + + switch (_state) { + /* drive master talon normally */ + case 0: + _rightMaster->Set(ControlMode::PercentOutput, axis, DemandType_ArbitraryFeedForward, -turn); + _leftMaster->Set(ControlMode::PercentOutput, axis, DemandType_ArbitraryFeedForward, turn); + if (bFireMp == true) { + /* go to MP logic */ + _state = 1; + } + break; + + /* fire the MP, and stop calling set() since that will cancel the MP */ + case 1: + _rightMaster->GetSensorCollection().SetQuadraturePosition(0); + _leftMaster->GetSensorCollection().SetQuadraturePosition(0); + _pidgey->SetYaw(0); + /* wait for 10 points to buffer in firmware, then transition to MP */ + _leftMaster->Follow(*_rightMaster, FollowerType_AuxOutput1); + _rightMaster->StartMotionProfile(*_bufferedStream, 10, ControlMode::MotionProfileArc); + _state = 2; + Instrum::PrintLine("MP started"); + break; + + /* wait for MP to finish */ + case 2: + if (_rightMaster->IsMotionProfileFinished()) { + Instrum::PrintLine("MP finished"); + _state = 3; + } + break; + + /* MP is finished, nothing to do */ + case 3: + break; + } + + /* print MP values */ + Instrum::Loop(bPrintValues, _rightMaster); +} + +void Robot::TestInit() {} +void Robot::TestPeriodic() {} + +void Robot::InitBuffer(const double profile[][3], int totalCnt, double rotations) +{ + bool forward = true; // set to false to drive in opposite direction of profile (not really needed + // since you can use negative numbers in profile). + + TrajectoryPoint point; // temp for for loop, since unused params are initialized + // automatically, you can alloc just one + + /* clear the buffer, in case it was used elsewhere */ + _bufferedStream->Clear(); + + double turnAmount = rotations * 8192.0; //8192 units per rotation for a pigeon + + + /* Insert every point into buffer, no limit on size */ + for (int i = 0; i < totalCnt; ++i) { + + double direction = forward ? +1 : -1; + double positionRot = profile[i][0]; + double velocityRPM = profile[i][1]; + int durationMilliseconds = (int) profile[i][2]; + + /* for each point, fill our structure and pass it to API */ + point.timeDur = durationMilliseconds; + point.position = direction * positionRot * 4096; // Convert Revolutions to + // Units + point.velocity = direction * velocityRPM * 4096 / 600.0; // Convert RPM to + // Units/100ms + + /** + * Here is where you specify the heading of the robot at each point. + * In this example we're linearly interpolating creating a segment of a circle to follow + */ + point.auxiliaryPos = turnAmount * ((double)i / (double)totalCnt); //Linearly interpolate the turn amount to do a circle + point.auxiliaryVel = 0; + + + point.profileSlotSelect0 = 0; /* which set of gains would you like to use [0,3]? */ + point.profileSlotSelect1 = 1; /* which set of gains would you like to use [0,3]? */ + point.zeroPos = (i == 0); /* set this to true on the first point */ + point.isLastPoint = ((i + 1) == totalCnt); /* set this to true on the last point */ + point.arbFeedFwd = 0; /* you can add a constant offset to add to PID[0] output here */ + + point.useAuxPID = true; /* Using auxiliary PID */ + _bufferedStream->Write(point); + } +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/C++/MotionProfileArc_Simple/src/main/include/FollowerProfileConfiguration.h b/C++/MotionProfileArc_Simple/src/main/include/FollowerProfileConfiguration.h new file mode 100644 index 00000000..3c27af51 --- /dev/null +++ b/C++/MotionProfileArc_Simple/src/main/include/FollowerProfileConfiguration.h @@ -0,0 +1,14 @@ +#pragma once + +#include "ctre/Phoenix.h" + +/* FollowerProfileConfiguration inherits TalonSRXConfiguration so it has all the default configs + the unique configs for the follower talon */ +class FollowerProfileConfiguration : public TalonSRXConfiguration +{ +public: + FollowerProfileConfiguration() : TalonSRXConfiguration() + { + primaryPID.selectedFeedbackSensor = FeedbackDevice::QuadEncoder; + neutralDeadband = 0.001; /* 0.1% super small for best low-speed control */ + } +}; \ No newline at end of file diff --git a/C++/MotionProfileArc_Simple/src/main/include/Instrum.h b/C++/MotionProfileArc_Simple/src/main/include/Instrum.h new file mode 100644 index 00000000..1ef3cc36 --- /dev/null +++ b/C++/MotionProfileArc_Simple/src/main/include/Instrum.h @@ -0,0 +1,60 @@ +#pragma once + +#include "ctre/Phoenix.h" +#include +#include +#include +/** + * Routines for printing to console (FRC Message log). + */ +class Instrum { +private: + static int _loops; + + static bool _bPrintValues; + +public: + static void PrintLine(std::string s) { + std::cout << s << std::endl; + } + + static void Loop(bool bPrintValues, TalonSRX *talon) { + if (!_bPrintValues && bPrintValues) { + /* user just pressed button, immediete print */ + _loops = 999; + } + /* if button is off, don't print */ + if (bPrintValues == false) { + /* reset so we don't print */ + _loops = 0; + } + /* save for next compare */ + _bPrintValues = bPrintValues; + + /* build string and print if button is down */ + if (++_loops >= 10) { + _loops = 0; + /* get status info */ + MotionProfileStatus status; + talon->GetMotionProfileStatus(status); + + std::stringstream line; + line << " topBufferRem: " << status.topBufferRem << "\n"; + line << " topBufferCnt: " << status.topBufferCnt << "\n"; + line << " btmBufferCnt: " << status.btmBufferCnt << "\n"; + line << " hasUnderrun: " << status.hasUnderrun << "\n"; + line << " isUnderrun: " << status.isUnderrun << "\n"; + line << " activePointValid: " << status.activePointValid << "\n"; + line << " isLast: " << status.isLast << "\n"; + line << " profileSlotSelect0: " << status.profileSlotSelect0 << "\n"; + line << " profileSlotSelect1: " << status.profileSlotSelect1 << "\n"; + line << " outputEnable: " << status.outputEnable << "\n"; + line << " timeDurMs: " << status.timeDurMs << "\n"; + + PrintLine(line.str()); + } + } +}; + +int Instrum::_loops = 0; +bool Instrum::_bPrintValues = false; diff --git a/C++/MotionProfileArc_Simple/src/main/include/MasterProfileConfiguration.h b/C++/MotionProfileArc_Simple/src/main/include/MasterProfileConfiguration.h new file mode 100644 index 00000000..251de484 --- /dev/null +++ b/C++/MotionProfileArc_Simple/src/main/include/MasterProfileConfiguration.h @@ -0,0 +1,48 @@ +#pragma once + +#include "ctre/Phoenix.h" + +/* MasterProfileConfiguration inherits TalonSRXConfiguration so it has all the default configs + the unique configs for the master talon */ +class MasterProfileConfiguration : public TalonSRXConfiguration +{ +public: + /* Constructor takes a talon and pigeon so it can reference it with their ID */ + MasterProfileConfiguration(TalonSRX *otherTalon, PigeonIMU *pigeon) : TalonSRXConfiguration() + { + /* Primary PID will be the sensor sum so it includes both sides */ + primaryPID.selectedFeedbackSensor = FeedbackDevice::SensorSum; + /* Auxiliary PID will be RemoteSensor0 which is the Pigeon */ + auxiliaryPID.selectedFeedbackSensor = FeedbackDevice::RemoteSensor1; + neutralDeadband = 0.001; /* 0.1% super small for best low-speed control */ + + /* Find these gains in Phoenix Tuner first and later put them here */ + slot0.kF = 0.35; + slot0.kP = 0.8; + slot0.kI = 0.0; + slot0.kD = 80; + slot0.integralZone = 400; + slot0.closedLoopPeakOutput = 1.0; + + slot1.kF = 0; + slot1.kP = 1.0; + slot1.kI = 0.0; + slot1.kD = 0.0; + slot1.integralZone = 400; + slot1.closedLoopPeakOutput = 0.5; + + /* Remote Sensor 0 is the other talon's quadrature encoder */ + remoteFilter0.remoteSensorSource = RemoteSensorSource::RemoteSensorSource_TalonSRX_SelectedSensor; + remoteFilter0.remoteSensorDeviceID = otherTalon->GetDeviceID(); + + /* Remote Sensor 1 is the Pigeon over CAN */ + remoteFilter1.remoteSensorSource = RemoteSensorSource::RemoteSensorSource_Pigeon_Yaw; + remoteFilter1.remoteSensorDeviceID = pigeon->GetDeviceNumber(); + + /* Configure sensor sum to be this quad encoder and the other talon's encoder */ + sum0Term = FeedbackDevice::QuadEncoder; + sum1Term = FeedbackDevice::RemoteSensor0; + + /* Configure auxPIDPolarity to match the drive train */ + auxPIDPolarity = false; + } +}; \ No newline at end of file diff --git a/C++/MotionProfileArc_Simple/src/main/include/MotionProfile.h b/C++/MotionProfileArc_Simple/src/main/include/MotionProfile.h new file mode 100644 index 00000000..bb3734fc --- /dev/null +++ b/C++/MotionProfileArc_Simple/src/main/include/MotionProfile.h @@ -0,0 +1,193 @@ +#pragma once +//Select the green highlighted cells and paste into a csv file. +//No need to copy the blank lines at the bottom. +//This can be pasted into an array for direct use in C++/Java. +// Position (rotations) Velocity (RPM) Duration (ms) +const int kMotionProfileSz =185; + +const double kMotionProfile[][3] = { +{0, 0 ,10}, +{4.76190476190476E-05, 0.571428571 ,10}, +{0.000214285714285714, 1.428571429 ,10}, +{0.000547619047619048, 2.571428571 ,10}, +{0.0010952380952381, 4 ,10}, +{0.0019047619047619, 5.714285714 ,10}, +{0.00302380952380952, 7.714285714 ,10}, +{0.0045, 10 ,10}, +{0.00638095238095238, 12.57142857 ,10}, +{0.00871428571428571, 15.42857143 ,10}, +{0.011547619047619, 18.57142857 ,10}, +{0.0149285714285714, 22 ,10}, +{0.0189047619047619, 25.71428571 ,10}, +{0.0235238095238095, 29.71428571 ,10}, +{0.0288333333333333, 34 ,10}, +{0.0348809523809524, 38.57142857 ,10}, +{0.0417142857142857, 43.42857143 ,10}, +{0.0493809523809524, 48.57142857 ,10}, +{0.0579285714285714, 54 ,10}, +{0.0674047619047619, 59.71428571 ,10}, +{0.0778571428571429, 65.71428571 ,10}, +{0.0893095238095238, 71.71428571 ,10}, +{0.101761904761905, 77.71428571 ,10}, +{0.115214285714286, 83.71428571 ,10}, +{0.129666666666667, 89.71428571 ,10}, +{0.145119047619048, 95.71428571 ,10}, +{0.161571428571429, 101.7142857 ,10}, +{0.17902380952381, 107.7142857 ,10}, +{0.19747619047619, 113.7142857 ,10}, +{0.216928571428571, 119.7142857 ,10}, +{0.237380952380952, 125.7142857 ,10}, +{0.258833333333333, 131.7142857 ,10}, +{0.281285714285714, 137.7142857 ,10}, +{0.304738095238095, 143.7142857 ,10}, +{0.329190476190476, 149.7142857 ,10}, +{0.354642857142857, 155.7142857 ,10}, +{0.381095238095238, 161.7142857 ,10}, +{0.408547619047619, 167.7142857 ,10}, +{0.437, 173.7142857 ,10}, +{0.466452380952381, 179.7142857 ,10}, +{0.496904761904762, 185.7142857 ,10}, +{0.528309523809524, 191.1428571 ,10}, +{0.560595238095238, 196.2857143 ,10}, +{0.593714285714286, 201.1428571 ,10}, +{0.627619047619048, 205.7142857 ,10}, +{0.662261904761905, 210 ,10}, +{0.697595238095238, 214 ,10}, +{0.733571428571429, 217.7142857 ,10}, +{0.770142857142857, 221.1428571 ,10}, +{0.807261904761905, 224.2857143 ,10}, +{0.844880952380952, 227.1428571 ,10}, +{0.882952380952381, 229.7142857 ,10}, +{0.921428571428571, 232 ,10}, +{0.960261904761905, 234 ,10}, +{0.999404761904762, 235.7142857 ,10}, +{1.03880952380952, 237.1428571 ,10}, +{1.07842857142857, 238.2857143 ,10}, +{1.11821428571429, 239.1428571 ,10}, +{1.15811904761905, 239.7142857 ,10}, +{1.19809523809524, 240 ,10}, +{1.23809523809524, 240 ,10}, +{1.27809523809524, 240 ,10}, +{1.31809523809524, 240 ,10}, +{1.35809523809524, 240 ,10}, +{1.39809523809524, 240 ,10}, +{1.43809523809524, 240 ,10}, +{1.47809523809524, 240 ,10}, +{1.51809523809524, 240 ,10}, +{1.55809523809524, 240 ,10}, +{1.59809523809524, 240 ,10}, +{1.63809523809524, 240 ,10}, +{1.67809523809524, 240 ,10}, +{1.71809523809524, 240 ,10}, +{1.75809523809524, 240 ,10}, +{1.79809523809524, 240 ,10}, +{1.83809523809524, 240 ,10}, +{1.87809523809524, 240 ,10}, +{1.91809523809524, 240 ,10}, +{1.95809523809524, 240 ,10}, +{1.99809523809524, 240 ,10}, +{2.03809523809524, 240 ,10}, +{2.07809523809524, 240 ,10}, +{2.11809523809524, 240 ,10}, +{2.15809523809524, 240 ,10}, +{2.19809523809524, 240 ,10}, +{2.23809523809524, 240 ,10}, +{2.27809523809524, 240 ,10}, +{2.31809523809524, 240 ,10}, +{2.35809523809524, 240 ,10}, +{2.39809523809524, 240 ,10}, +{2.43809523809524, 240 ,10}, +{2.47809523809524, 240 ,10}, +{2.51809523809524, 240 ,10}, +{2.55809523809524, 240 ,10}, +{2.59809523809524, 240 ,10}, +{2.63809523809524, 240 ,10}, +{2.67809523809524, 240 ,10}, +{2.71809523809524, 240 ,10}, +{2.75809523809524, 240 ,10}, +{2.79809523809524, 240 ,10}, +{2.83809523809524, 240 ,10}, +{2.87809523809524, 240 ,10}, +{2.91809523809524, 240 ,10}, +{2.95809523809524, 240 ,10}, +{2.99809523809524, 240 ,10}, +{3.03809523809524, 240 ,10}, +{3.07809523809524, 240 ,10}, +{3.11809523809524, 240 ,10}, +{3.15809523809524, 240 ,10}, +{3.19809523809524, 240 ,10}, +{3.23809523809524, 240 ,10}, +{3.27809523809524, 240 ,10}, +{3.31809523809524, 240 ,10}, +{3.35809523809524, 240 ,10}, +{3.39809523809524, 240 ,10}, +{3.43809523809524, 240 ,10}, +{3.47809523809524, 240 ,10}, +{3.51809523809524, 240 ,10}, +{3.55809523809524, 240 ,10}, +{3.59809523809524, 240 ,10}, +{3.63809523809524, 240 ,10}, +{3.67809523809524, 240 ,10}, +{3.71809523809524, 240 ,10}, +{3.75809523809524, 240 ,10}, +{3.79809523809524, 240 ,10}, +{3.83809523809524, 240 ,10}, +{3.87804761904762, 239.4285714 ,10}, +{3.91788095238095, 238.5714286 ,10}, +{3.95754761904762, 237.4285714 ,10}, +{3.997, 236 ,10}, +{4.03619047619048, 234.2857143 ,10}, +{4.07507142857143, 232.2857143 ,10}, +{4.11359523809524, 230 ,10}, +{4.15171428571429, 227.4285714 ,10}, +{4.18938095238095, 224.5714286 ,10}, +{4.22654761904762, 221.4285714 ,10}, +{4.26316666666667, 218 ,10}, +{4.29919047619048, 214.2857143 ,10}, +{4.33457142857143, 210.2857143 ,10}, +{4.36926190476191, 206 ,10}, +{4.40321428571429, 201.4285714 ,10}, +{4.43638095238095, 196.5714286 ,10}, +{4.46871428571429, 191.4285714 ,10}, +{4.50016666666667, 186 ,10}, +{4.53069047619048, 180.2857143 ,10}, +{4.5602380952381, 174.2857143 ,10}, +{4.58878571428572, 168.2857143 ,10}, +{4.61633333333334, 162.2857143 ,10}, +{4.64288095238095, 156.2857143 ,10}, +{4.66842857142857, 150.2857143 ,10}, +{4.69297619047619, 144.2857143 ,10}, +{4.71652380952381, 138.2857143 ,10}, +{4.73907142857143, 132.2857143 ,10}, +{4.76061904761905, 126.2857143 ,10}, +{4.78116666666667, 120.2857143 ,10}, +{4.80071428571429, 114.2857143 ,10}, +{4.81926190476191, 108.2857143 ,10}, +{4.83680952380953, 102.2857143 ,10}, +{4.85335714285714, 96.28571429 ,10}, +{4.86890476190476, 90.28571429 ,10}, +{4.88345238095238, 84.28571429 ,10}, +{4.897, 78.28571429 ,10}, +{4.90954761904762, 72.28571429 ,10}, +{4.92109523809524, 66.28571429 ,10}, +{4.93164285714286, 60.28571429 ,10}, +{4.94119047619048, 54.28571429 ,10}, +{4.94978571428572, 48.85714286 ,10}, +{4.9575, 43.71428571 ,10}, +{4.96438095238095, 38.85714286 ,10}, +{4.97047619047619, 34.28571429 ,10}, +{4.97583333333333, 30 ,10}, +{4.9805, 26 ,10}, +{4.98452380952381, 22.28571429 ,10}, +{4.98795238095238, 18.85714286 ,10}, +{4.99083333333334, 15.71428571 ,10}, +{4.99321428571429, 12.85714286 ,10}, +{4.99514285714286, 10.28571429 ,10}, +{4.99666666666667, 8 ,10}, +{4.99783333333334, 6 ,10}, +{4.99869047619048, 4.285714286 ,10}, +{4.99928571428572, 2.857142857 ,10}, +{4.99966666666667, 1.714285714 ,10}, +{4.99988095238095, 0.857142857 ,10}, +{4.99997619047619, 0.285714286 ,10}, +{5, 0 ,10}}; \ No newline at end of file diff --git a/C++/MotionProfileArc_Simple/src/main/include/PlotThread.h b/C++/MotionProfileArc_Simple/src/main/include/PlotThread.h new file mode 100644 index 00000000..ae7f7ad2 --- /dev/null +++ b/C++/MotionProfileArc_Simple/src/main/include/PlotThread.h @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include "ctre/Phoenix.h" + +class PlotThread +{ +private: + std::thread *_thread; +public: + PlotThread(TalonSRX *talon) + { + _thread = new std::thread(Run, talon); + } + + static void Run(TalonSRX *_talon) + { + /** + * Speed up network tables, this is a test project so eat up all of the network + * possible for the purpose of this test. + */ + + while (true) { + /* Yield for a Ms or so - this is not meant to be accurate */ + usleep(1000); + + /* Grab the latest signal update from our 1ms frame update */ + double sen_pos = _talon->GetSelectedSensorPosition(0); + double sen_vel = _talon->GetSelectedSensorVelocity(0); + double trgt_pos = _talon->GetActiveTrajectoryPosition(0); + double trgt_vel = _talon->GetActiveTrajectoryVelocity(0); + double trgt_arbF = _talon->GetActiveTrajectoryArbFeedFwd(0); + frc::SmartDashboard::PutNumber("sen_pos", sen_pos); + frc::SmartDashboard::PutNumber("sen_vel", sen_vel); + frc::SmartDashboard::PutNumber("trgt_pos", trgt_pos); + frc::SmartDashboard::PutNumber("trgt_vel", trgt_vel); + frc::SmartDashboard::PutNumber("trgt_arbF", trgt_arbF); + } + } +}; diff --git a/C++/MotionProfileArc_Simple/src/main/include/Robot.h b/C++/MotionProfileArc_Simple/src/main/include/Robot.h new file mode 100644 index 00000000..1992c385 --- /dev/null +++ b/C++/MotionProfileArc_Simple/src/main/include/Robot.h @@ -0,0 +1,48 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include "ctre/Phoenix.h" +#include "PlotThread.h" +#include "MasterProfileConfiguration.h" +#include "FollowerProfileConfiguration.h" + +class Robot : public frc::TimedRobot +{ +public: + void RobotInit() override; + + void AutonomousInit() override; + void AutonomousPeriodic() override; + + void TeleopInit() override; + void TeleopPeriodic() override; + + void TestInit() override; + void TestPeriodic() override; + +private: + int _state; + + TalonSRX *_rightMaster; + TalonSRX *_leftMaster; + + MasterProfileConfiguration *_masterConfig; + FollowerProfileConfiguration *_followConfig; + + PigeonIMU *_pidgey; + + frc::Joystick *_joystick; + + BufferedTrajectoryPointStream *_bufferedStream; + PlotThread *_plotThread; + + void InitBuffer(const double profile[][3], int totalCnt, double rotations); +}; diff --git a/C++/MotionProfileArc_Simple/src/test/cpp/main.cpp b/C++/MotionProfileArc_Simple/src/test/cpp/main.cpp new file mode 100644 index 00000000..b8b23d23 --- /dev/null +++ b/C++/MotionProfileArc_Simple/src/test/cpp/main.cpp @@ -0,0 +1,10 @@ +#include + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/C++/MotionProfileArc_Simple/vendordeps/Phoenix.json b/C++/MotionProfileArc_Simple/vendordeps/Phoenix.json new file mode 100644 index 00000000..2f05720b --- /dev/null +++ b/C++/MotionProfileArc_Simple/vendordeps/Phoenix.json @@ -0,0 +1,135 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.13.0", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.13.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.13.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.13.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.13.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.13.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.13.0", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.13.0", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.13.0", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers" + } + ] +} \ No newline at end of file diff --git a/C++/MotionProfile_Simple/.wpilib/wpilib_preferences.json b/C++/MotionProfile_Simple/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..d35f186c --- /dev/null +++ b/C++/MotionProfile_Simple/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": true, + "currentLanguage": "cpp", + "projectYear": "2019", + "teamNumber": 7762 +} \ No newline at end of file diff --git a/C++/MotionProfile_Simple/build.gradle b/C++/MotionProfile_Simple/build.gradle new file mode 100644 index 00000000..2972dd43 --- /dev/null +++ b/C++/MotionProfile_Simple/build.gradle @@ -0,0 +1,87 @@ +plugins { + id "cpp" + id "google-test-test-suite" + id "edu.wpi.first.GradleRIO" version "2019.1.1" +} + +// 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 { + frcNativeArtifact('frcCpp') { + targets << "roborio" + component = 'frcUserProgram' + // 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 include the src folder in the include directories passed +// to the compiler. Some eclipse project imports depend on this behavior. +// We recommend leaving this disabled if possible. Note for eclipse project +// imports this is enabled by default. For new projects, its disabled +def includeSrcInIncludeRoot = false + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +model { + components { + frcUserProgram(NativeExecutableSpec) { + targetPlatform wpi.platforms.roborio + if (includeDesktopSupport) { + targetPlatform wpi.platforms.desktop + } + + sources.cpp { + source { + srcDir 'src/main/cpp' + include '**/*.cpp', '**/*.cc' + } + exportedHeaders { + srcDir 'src/main/include' + if (includeSrcInIncludeRoot) { + srcDir 'src/main/cpp' + } + } + } + + // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. + useLibrary(it, "wpilib") + wpi.deps.vendor.cpp(it) + } + } + testSuites { + frcUserProgramTest(GoogleTestTestSuiteSpec) { + testing $.components.frcUserProgram + + sources.cpp { + source { + srcDir 'src/test/cpp' + include '**/*.cpp' + } + } + + useLibrary(it, "wpilib", "googletest") + wpi.deps.vendor.cpp(it) + } + } +} diff --git a/C++/MotionProfile_Simple/gradle/wrapper/gradle-wrapper.jar b/C++/MotionProfile_Simple/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 00000000..457aad0d Binary files /dev/null and b/C++/MotionProfile_Simple/gradle/wrapper/gradle-wrapper.jar differ diff --git a/C++/MotionProfile_Simple/gradle/wrapper/gradle-wrapper.properties b/C++/MotionProfile_Simple/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..d08253ca --- /dev/null +++ b/C++/MotionProfile_Simple/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/C++/MotionProfile_Simple/gradlew b/C++/MotionProfile_Simple/gradlew new file mode 100644 index 00000000..af6708ff --- /dev/null +++ b/C++/MotionProfile_Simple/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/C++/MotionProfile_Simple/gradlew.bat b/C++/MotionProfile_Simple/gradlew.bat new file mode 100644 index 00000000..6d57edc7 --- /dev/null +++ b/C++/MotionProfile_Simple/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/C++/MotionProfile_Simple/settings.gradle b/C++/MotionProfile_Simple/settings.gradle new file mode 100644 index 00000000..b0f4d48a --- /dev/null +++ b/C++/MotionProfile_Simple/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/C++/MotionProfile_Simple/src/main/cpp/Robot.cpp b/C++/MotionProfile_Simple/src/main/cpp/Robot.cpp new file mode 100644 index 00000000..1ae5791d --- /dev/null +++ b/C++/MotionProfile_Simple/src/main/cpp/Robot.cpp @@ -0,0 +1,127 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "Robot.h" +#include "MotionProfile.h" +#include "Instrum.h" + +void Robot::RobotInit() +{ + /* Construct global variables being used */ + _master = new TalonSRX(0); + _joy = new frc::Joystick(0); + _bufferedStream = new BufferedTrajectoryPointStream(); + + _plotThread = new PlotThread(_master); + + /* Initialize buffer with MotionProfile */ + InitBuffer(kMotionProfile, kMotionProfileSz); + _state = 0; + + + _configuration = new MotionProfileConfiguration(); + + _master->ConfigAllSettings(*_configuration); + _master->SetSensorPhase(true); //Flip this if you need to for your robot + _master->SetInverted(false); //Flip this if you need to for your robot +} + +void Robot::AutonomousInit() {} +void Robot::AutonomousPeriodic() {} + +void Robot::TeleopInit() {} +void Robot::TeleopPeriodic() +{ + /* get joystick button and stick */ + bool bPrintValues = _joy->GetRawButton(2); + bool bFireMp = _joy->GetRawButton(1); + double axis = _joy->GetRawAxis(1); + + /* if button is up, just drive the motor in PercentOutput */ + if (bFireMp == false) { + _state = 0; + } + + switch (_state) { + /* drive master talon normally */ + case 0: + _master->Set(ControlMode::PercentOutput, axis); + if (bFireMp == true) { + /* go to MP logic */ + _state = 1; + } + break; + + /* fire the MP, and stop calling set() since that will cancel the MP */ + case 1: + /* wait for 10 points to buffer in firmware, then transition to MP */ + _master->StartMotionProfile(*_bufferedStream, 10, ControlMode::MotionProfile); + _state = 2; + Instrum::PrintLine("MP started"); + break; + + /* wait for MP to finish */ + case 2: + if (_master->IsMotionProfileFinished()) { + Instrum::PrintLine("MP finished"); + _state = 3; + } + break; + + /* MP is finished, nothing to do */ + case 3: + break; + } + + /* print MP values */ + Instrum::Loop(bPrintValues, _master); +} + +void Robot::TestInit() {} +void Robot::TestPeriodic() {} + + +void Robot::InitBuffer(const double profile[][3], int totalCnt) +{ + bool forward = true; // set to false to drive in opposite direction of profile (not really needed + // since you can use negative numbers in profile). + + TrajectoryPoint point; // temp for for loop, since unused params are initialized + // automatically, you can alloc just one + + /* clear the buffer, in case it was used elsewhere */ + _bufferedStream->Clear(); + + /* Insert every point into buffer, no limit on size */ + for (int i = 0; i < totalCnt; ++i) { + + double direction = forward ? +1 : -1; + double positionRot = profile[i][0]; + double velocityRPM = profile[i][1]; + int durationMilliseconds = (int) profile[i][2]; + + /* for each point, fill our structure and pass it to API */ + point.timeDur = durationMilliseconds; + point.position = direction * positionRot * 4096; // Convert Revolutions to + // Units + point.velocity = direction * velocityRPM * 4096 / 600.0; // Convert RPM to + // Units/100ms + point.auxiliaryPos = 0; + point.auxiliaryVel = 0; + point.profileSlotSelect0 = 0; /* which set of gains would you like to use [0,3]? */ + point.profileSlotSelect1 = 0; /* which set of gains would you like to use [0,3]? */ + point.zeroPos = (i == 0); /* set this to true on the first point */ + point.isLastPoint = ((i + 1) == totalCnt); /* set this to true on the last point */ + point.arbFeedFwd = 0; /* you can add a constant offset to add to PID[0] output here */ + + _bufferedStream->Write(point); + } +} + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/C++/MotionProfile_Simple/src/main/deploy/example.txt b/C++/MotionProfile_Simple/src/main/deploy/example.txt new file mode 100644 index 00000000..1c240824 --- /dev/null +++ b/C++/MotionProfile_Simple/src/main/deploy/example.txt @@ -0,0 +1,4 @@ +Files placed in this directory will be deployed to the RoboRIO into the +'deploy' directory in the home folder. Use the 'frc::GetFilePath' function from +the 'frc/FileUtilities.h' header to get a proper path relative to the deploy +directory. \ No newline at end of file diff --git a/C++/MotionProfile_Simple/src/main/include/Instrum.h b/C++/MotionProfile_Simple/src/main/include/Instrum.h new file mode 100644 index 00000000..1ef3cc36 --- /dev/null +++ b/C++/MotionProfile_Simple/src/main/include/Instrum.h @@ -0,0 +1,60 @@ +#pragma once + +#include "ctre/Phoenix.h" +#include +#include +#include +/** + * Routines for printing to console (FRC Message log). + */ +class Instrum { +private: + static int _loops; + + static bool _bPrintValues; + +public: + static void PrintLine(std::string s) { + std::cout << s << std::endl; + } + + static void Loop(bool bPrintValues, TalonSRX *talon) { + if (!_bPrintValues && bPrintValues) { + /* user just pressed button, immediete print */ + _loops = 999; + } + /* if button is off, don't print */ + if (bPrintValues == false) { + /* reset so we don't print */ + _loops = 0; + } + /* save for next compare */ + _bPrintValues = bPrintValues; + + /* build string and print if button is down */ + if (++_loops >= 10) { + _loops = 0; + /* get status info */ + MotionProfileStatus status; + talon->GetMotionProfileStatus(status); + + std::stringstream line; + line << " topBufferRem: " << status.topBufferRem << "\n"; + line << " topBufferCnt: " << status.topBufferCnt << "\n"; + line << " btmBufferCnt: " << status.btmBufferCnt << "\n"; + line << " hasUnderrun: " << status.hasUnderrun << "\n"; + line << " isUnderrun: " << status.isUnderrun << "\n"; + line << " activePointValid: " << status.activePointValid << "\n"; + line << " isLast: " << status.isLast << "\n"; + line << " profileSlotSelect0: " << status.profileSlotSelect0 << "\n"; + line << " profileSlotSelect1: " << status.profileSlotSelect1 << "\n"; + line << " outputEnable: " << status.outputEnable << "\n"; + line << " timeDurMs: " << status.timeDurMs << "\n"; + + PrintLine(line.str()); + } + } +}; + +int Instrum::_loops = 0; +bool Instrum::_bPrintValues = false; diff --git a/C++/MotionProfile_Simple/src/main/include/MotionProfile.h b/C++/MotionProfile_Simple/src/main/include/MotionProfile.h new file mode 100644 index 00000000..bb3734fc --- /dev/null +++ b/C++/MotionProfile_Simple/src/main/include/MotionProfile.h @@ -0,0 +1,193 @@ +#pragma once +//Select the green highlighted cells and paste into a csv file. +//No need to copy the blank lines at the bottom. +//This can be pasted into an array for direct use in C++/Java. +// Position (rotations) Velocity (RPM) Duration (ms) +const int kMotionProfileSz =185; + +const double kMotionProfile[][3] = { +{0, 0 ,10}, +{4.76190476190476E-05, 0.571428571 ,10}, +{0.000214285714285714, 1.428571429 ,10}, +{0.000547619047619048, 2.571428571 ,10}, +{0.0010952380952381, 4 ,10}, +{0.0019047619047619, 5.714285714 ,10}, +{0.00302380952380952, 7.714285714 ,10}, +{0.0045, 10 ,10}, +{0.00638095238095238, 12.57142857 ,10}, +{0.00871428571428571, 15.42857143 ,10}, +{0.011547619047619, 18.57142857 ,10}, +{0.0149285714285714, 22 ,10}, +{0.0189047619047619, 25.71428571 ,10}, +{0.0235238095238095, 29.71428571 ,10}, +{0.0288333333333333, 34 ,10}, +{0.0348809523809524, 38.57142857 ,10}, +{0.0417142857142857, 43.42857143 ,10}, +{0.0493809523809524, 48.57142857 ,10}, +{0.0579285714285714, 54 ,10}, +{0.0674047619047619, 59.71428571 ,10}, +{0.0778571428571429, 65.71428571 ,10}, +{0.0893095238095238, 71.71428571 ,10}, +{0.101761904761905, 77.71428571 ,10}, +{0.115214285714286, 83.71428571 ,10}, +{0.129666666666667, 89.71428571 ,10}, +{0.145119047619048, 95.71428571 ,10}, +{0.161571428571429, 101.7142857 ,10}, +{0.17902380952381, 107.7142857 ,10}, +{0.19747619047619, 113.7142857 ,10}, +{0.216928571428571, 119.7142857 ,10}, +{0.237380952380952, 125.7142857 ,10}, +{0.258833333333333, 131.7142857 ,10}, +{0.281285714285714, 137.7142857 ,10}, +{0.304738095238095, 143.7142857 ,10}, +{0.329190476190476, 149.7142857 ,10}, +{0.354642857142857, 155.7142857 ,10}, +{0.381095238095238, 161.7142857 ,10}, +{0.408547619047619, 167.7142857 ,10}, +{0.437, 173.7142857 ,10}, +{0.466452380952381, 179.7142857 ,10}, +{0.496904761904762, 185.7142857 ,10}, +{0.528309523809524, 191.1428571 ,10}, +{0.560595238095238, 196.2857143 ,10}, +{0.593714285714286, 201.1428571 ,10}, +{0.627619047619048, 205.7142857 ,10}, +{0.662261904761905, 210 ,10}, +{0.697595238095238, 214 ,10}, +{0.733571428571429, 217.7142857 ,10}, +{0.770142857142857, 221.1428571 ,10}, +{0.807261904761905, 224.2857143 ,10}, +{0.844880952380952, 227.1428571 ,10}, +{0.882952380952381, 229.7142857 ,10}, +{0.921428571428571, 232 ,10}, +{0.960261904761905, 234 ,10}, +{0.999404761904762, 235.7142857 ,10}, +{1.03880952380952, 237.1428571 ,10}, +{1.07842857142857, 238.2857143 ,10}, +{1.11821428571429, 239.1428571 ,10}, +{1.15811904761905, 239.7142857 ,10}, +{1.19809523809524, 240 ,10}, +{1.23809523809524, 240 ,10}, +{1.27809523809524, 240 ,10}, +{1.31809523809524, 240 ,10}, +{1.35809523809524, 240 ,10}, +{1.39809523809524, 240 ,10}, +{1.43809523809524, 240 ,10}, +{1.47809523809524, 240 ,10}, +{1.51809523809524, 240 ,10}, +{1.55809523809524, 240 ,10}, +{1.59809523809524, 240 ,10}, +{1.63809523809524, 240 ,10}, +{1.67809523809524, 240 ,10}, +{1.71809523809524, 240 ,10}, +{1.75809523809524, 240 ,10}, +{1.79809523809524, 240 ,10}, +{1.83809523809524, 240 ,10}, +{1.87809523809524, 240 ,10}, +{1.91809523809524, 240 ,10}, +{1.95809523809524, 240 ,10}, +{1.99809523809524, 240 ,10}, +{2.03809523809524, 240 ,10}, +{2.07809523809524, 240 ,10}, +{2.11809523809524, 240 ,10}, +{2.15809523809524, 240 ,10}, +{2.19809523809524, 240 ,10}, +{2.23809523809524, 240 ,10}, +{2.27809523809524, 240 ,10}, +{2.31809523809524, 240 ,10}, +{2.35809523809524, 240 ,10}, +{2.39809523809524, 240 ,10}, +{2.43809523809524, 240 ,10}, +{2.47809523809524, 240 ,10}, +{2.51809523809524, 240 ,10}, +{2.55809523809524, 240 ,10}, +{2.59809523809524, 240 ,10}, +{2.63809523809524, 240 ,10}, +{2.67809523809524, 240 ,10}, +{2.71809523809524, 240 ,10}, +{2.75809523809524, 240 ,10}, +{2.79809523809524, 240 ,10}, +{2.83809523809524, 240 ,10}, +{2.87809523809524, 240 ,10}, +{2.91809523809524, 240 ,10}, +{2.95809523809524, 240 ,10}, +{2.99809523809524, 240 ,10}, +{3.03809523809524, 240 ,10}, +{3.07809523809524, 240 ,10}, +{3.11809523809524, 240 ,10}, +{3.15809523809524, 240 ,10}, +{3.19809523809524, 240 ,10}, +{3.23809523809524, 240 ,10}, +{3.27809523809524, 240 ,10}, +{3.31809523809524, 240 ,10}, +{3.35809523809524, 240 ,10}, +{3.39809523809524, 240 ,10}, +{3.43809523809524, 240 ,10}, +{3.47809523809524, 240 ,10}, +{3.51809523809524, 240 ,10}, +{3.55809523809524, 240 ,10}, +{3.59809523809524, 240 ,10}, +{3.63809523809524, 240 ,10}, +{3.67809523809524, 240 ,10}, +{3.71809523809524, 240 ,10}, +{3.75809523809524, 240 ,10}, +{3.79809523809524, 240 ,10}, +{3.83809523809524, 240 ,10}, +{3.87804761904762, 239.4285714 ,10}, +{3.91788095238095, 238.5714286 ,10}, +{3.95754761904762, 237.4285714 ,10}, +{3.997, 236 ,10}, +{4.03619047619048, 234.2857143 ,10}, +{4.07507142857143, 232.2857143 ,10}, +{4.11359523809524, 230 ,10}, +{4.15171428571429, 227.4285714 ,10}, +{4.18938095238095, 224.5714286 ,10}, +{4.22654761904762, 221.4285714 ,10}, +{4.26316666666667, 218 ,10}, +{4.29919047619048, 214.2857143 ,10}, +{4.33457142857143, 210.2857143 ,10}, +{4.36926190476191, 206 ,10}, +{4.40321428571429, 201.4285714 ,10}, +{4.43638095238095, 196.5714286 ,10}, +{4.46871428571429, 191.4285714 ,10}, +{4.50016666666667, 186 ,10}, +{4.53069047619048, 180.2857143 ,10}, +{4.5602380952381, 174.2857143 ,10}, +{4.58878571428572, 168.2857143 ,10}, +{4.61633333333334, 162.2857143 ,10}, +{4.64288095238095, 156.2857143 ,10}, +{4.66842857142857, 150.2857143 ,10}, +{4.69297619047619, 144.2857143 ,10}, +{4.71652380952381, 138.2857143 ,10}, +{4.73907142857143, 132.2857143 ,10}, +{4.76061904761905, 126.2857143 ,10}, +{4.78116666666667, 120.2857143 ,10}, +{4.80071428571429, 114.2857143 ,10}, +{4.81926190476191, 108.2857143 ,10}, +{4.83680952380953, 102.2857143 ,10}, +{4.85335714285714, 96.28571429 ,10}, +{4.86890476190476, 90.28571429 ,10}, +{4.88345238095238, 84.28571429 ,10}, +{4.897, 78.28571429 ,10}, +{4.90954761904762, 72.28571429 ,10}, +{4.92109523809524, 66.28571429 ,10}, +{4.93164285714286, 60.28571429 ,10}, +{4.94119047619048, 54.28571429 ,10}, +{4.94978571428572, 48.85714286 ,10}, +{4.9575, 43.71428571 ,10}, +{4.96438095238095, 38.85714286 ,10}, +{4.97047619047619, 34.28571429 ,10}, +{4.97583333333333, 30 ,10}, +{4.9805, 26 ,10}, +{4.98452380952381, 22.28571429 ,10}, +{4.98795238095238, 18.85714286 ,10}, +{4.99083333333334, 15.71428571 ,10}, +{4.99321428571429, 12.85714286 ,10}, +{4.99514285714286, 10.28571429 ,10}, +{4.99666666666667, 8 ,10}, +{4.99783333333334, 6 ,10}, +{4.99869047619048, 4.285714286 ,10}, +{4.99928571428572, 2.857142857 ,10}, +{4.99966666666667, 1.714285714 ,10}, +{4.99988095238095, 0.857142857 ,10}, +{4.99997619047619, 0.285714286 ,10}, +{5, 0 ,10}}; \ No newline at end of file diff --git a/C++/MotionProfile_Simple/src/main/include/MotionProfileConfiguration.h b/C++/MotionProfile_Simple/src/main/include/MotionProfileConfiguration.h new file mode 100644 index 00000000..68d3f173 --- /dev/null +++ b/C++/MotionProfile_Simple/src/main/include/MotionProfileConfiguration.h @@ -0,0 +1,21 @@ +#pragma once + +#include "ctre/Phoenix.h" + +/* Class that inherits TalonSRXConfiguration so it has all the default configs + modified the configs it cares about */ +class MotionProfileConfiguration : public TalonSRXConfiguration +{ +public: + MotionProfileConfiguration() : TalonSRXConfiguration() + { + primaryPID.selectedFeedbackSensor = QuadEncoder; + neutralDeadband = 0.001; /* 0.1% super small for best low-speed control */ + + slot0.kF = 1023.0 / 68000.0; + slot0.kP = 1.0; + slot0.kI = 0.0; + slot0.kD = 0.0; + slot0.integralZone = 400; + slot0.closedLoopPeakOutput = 1.0; + } +}; \ No newline at end of file diff --git a/C++/MotionProfile_Simple/src/main/include/PlotThread.h b/C++/MotionProfile_Simple/src/main/include/PlotThread.h new file mode 100644 index 00000000..ae7f7ad2 --- /dev/null +++ b/C++/MotionProfile_Simple/src/main/include/PlotThread.h @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include "ctre/Phoenix.h" + +class PlotThread +{ +private: + std::thread *_thread; +public: + PlotThread(TalonSRX *talon) + { + _thread = new std::thread(Run, talon); + } + + static void Run(TalonSRX *_talon) + { + /** + * Speed up network tables, this is a test project so eat up all of the network + * possible for the purpose of this test. + */ + + while (true) { + /* Yield for a Ms or so - this is not meant to be accurate */ + usleep(1000); + + /* Grab the latest signal update from our 1ms frame update */ + double sen_pos = _talon->GetSelectedSensorPosition(0); + double sen_vel = _talon->GetSelectedSensorVelocity(0); + double trgt_pos = _talon->GetActiveTrajectoryPosition(0); + double trgt_vel = _talon->GetActiveTrajectoryVelocity(0); + double trgt_arbF = _talon->GetActiveTrajectoryArbFeedFwd(0); + frc::SmartDashboard::PutNumber("sen_pos", sen_pos); + frc::SmartDashboard::PutNumber("sen_vel", sen_vel); + frc::SmartDashboard::PutNumber("trgt_pos", trgt_pos); + frc::SmartDashboard::PutNumber("trgt_vel", trgt_vel); + frc::SmartDashboard::PutNumber("trgt_arbF", trgt_arbF); + } + } +}; diff --git a/C++/MotionProfile_Simple/src/main/include/Robot.h b/C++/MotionProfile_Simple/src/main/include/Robot.h new file mode 100644 index 00000000..38eb6c1e --- /dev/null +++ b/C++/MotionProfile_Simple/src/main/include/Robot.h @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "ctre/Phoenix.h" +#include "MotionProfileConfiguration.h" +#include "PlotThread.h" + +class Robot : public frc::TimedRobot +{ +public: + void RobotInit() override; + + void AutonomousInit() override; + void AutonomousPeriodic() override; + + void TeleopInit() override; + void TeleopPeriodic() override; + + void TestInit() override; + void TestPeriodic() override; + +private: + int _state; + TalonSRX *_master; + frc::Joystick *_joy; + BufferedTrajectoryPointStream *_bufferedStream; + MotionProfileConfiguration *_configuration; + PlotThread *_plotThread; + + void InitBuffer(const double profile[][3], int totalCnt); +}; diff --git a/C++/MotionProfile_Simple/src/test/cpp/main.cpp b/C++/MotionProfile_Simple/src/test/cpp/main.cpp new file mode 100644 index 00000000..b8b23d23 --- /dev/null +++ b/C++/MotionProfile_Simple/src/test/cpp/main.cpp @@ -0,0 +1,10 @@ +#include + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + HAL_Initialize(500, 0); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/C++/MotionProfile_Simple/vendordeps/Phoenix.json b/C++/MotionProfile_Simple/vendordeps/Phoenix.json new file mode 100644 index 00000000..2f05720b --- /dev/null +++ b/C++/MotionProfile_Simple/vendordeps/Phoenix.json @@ -0,0 +1,135 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.13.0", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.13.0" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.13.0" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.13.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.13.0", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.13.0", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.13.0", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.13.0", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.13.0", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.13.0", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers" + } + ] +} \ No newline at end of file diff --git a/Java/MotionMagic/src/main/java/frc/robot/Robot.java b/Java/MotionMagic/src/main/java/frc/robot/Robot.java index be2235d4..7518eb17 100644 --- a/Java/MotionMagic/src/main/java/frc/robot/Robot.java +++ b/Java/MotionMagic/src/main/java/frc/robot/Robot.java @@ -43,6 +43,9 @@ * Controls: * Button 1: When held, put Talon in Motion Magic mode and allow Talon to drive [-10, 10] * rotations. + * Button 2: When pushed, the selected feedback sensor gets zero'd + * Button 5(Left shoulder): When pushed, will decrement the smoothing of the motion magic down to 0 + * Button 6(Right shoulder): When pushed, will increment the smoothing of the motion magic up to 8 * Left Joystick Y-Axis: * + Percent Output: Throttle Talon SRX forward and reverse, use to confirm hardware setup. * + Motion Maigic: SErvo Talon SRX forward and reverse, [-10, 10] rotations. @@ -68,17 +71,20 @@ public class Robot extends TimedRobot { /* Hardware */ - TalonSRX _talon = new TalonSRX(1); + TalonSRX _talon = new TalonSRX(12); Joystick _joy = new Joystick(0); /* create some followers */ - BaseMotorController _follower1 = new TalonSRX(0); + BaseMotorController _follower1 = new TalonSRX(2); BaseMotorController _follower2 = new VictorSPX(0); BaseMotorController _follower3 = new VictorSPX(1); /* Used to build string throughout loop */ StringBuilder _sb = new StringBuilder(); + /* Create integer to keep track of smoothing variable */ + int _smoothing = 0; + public void robotInit() { /* setup some followers */ _follower1.configFactoryDefault(); @@ -102,7 +108,7 @@ public void robotInit() { * Phase sensor to have positive increment when driving Talon Forward (Green LED) */ _talon.setSensorPhase(true); - _talon.setInverted(false); + _talon.setInverted(true); /* Set relevant frame periods to be at least as fast as periodic rate */ _talon.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, Constants.kTimeoutMs); @@ -122,8 +128,8 @@ public void robotInit() { _talon.config_kD(Constants.kSlotIdx, Constants.kGains.kD, Constants.kTimeoutMs); /* Set acceleration and vcruise velocity - see documentation */ - _talon.configMotionCruiseVelocity(15000, Constants.kTimeoutMs); - _talon.configMotionAcceleration(6000, Constants.kTimeoutMs); + _talon.configMotionCruiseVelocity(3000, Constants.kTimeoutMs); + _talon.configMotionAcceleration(500, Constants.kTimeoutMs); /* Zero the sensor */ _talon.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); @@ -167,6 +173,32 @@ public void teleopPeriodic() { _talon.set(ControlMode.PercentOutput, leftYstick); } + if(_joy.getRawButton(2)) + { + /* Clear sensor positions */ + _talon.getSensorCollection().setQuadraturePosition(0, 0); + + System.out.println("Voltage is: " + _talon.getBusVoltage()); + } + + if(_joy.getRawButtonPressed(5)) + { + /* Decrease smoothing */ + _smoothing--; + if(_smoothing < 0) _smoothing = 0; + _talon.configMotionSCurveStrength(_smoothing); + + System.out.println("Smoothing is set to: " + _smoothing); + } + if(_joy.getRawButtonPressed(6)) + { + /* Increase smoothing */ + _smoothing++; + if(_smoothing > 8) _smoothing = 8; + _talon.configMotionSCurveStrength(_smoothing); + + System.out.println("Smoothing is set to: " + _smoothing); + } /* Instrumentation */ Instrum.Process(_talon, _sb); diff --git a/Java/MotionMagic_AuxFeedForward/src/main/java/frc/robot/Robot.java b/Java/MotionMagic_AuxFeedForward/src/main/java/frc/robot/Robot.java index ba29bb27..a7c03db3 100644 --- a/Java/MotionMagic_AuxFeedForward/src/main/java/frc/robot/Robot.java +++ b/Java/MotionMagic_AuxFeedForward/src/main/java/frc/robot/Robot.java @@ -41,6 +41,8 @@ * Button 2: When pressed, toggle between Arcade Drive and Motion Magic * When toggling into Motion Magic, the current heading is saved and used as the * closed loop target. Can be changed by toggling out and in again. + * Button 5(Left shoulder): When pushed, will decrement the smoothing of the motion magic down to 0 + * Button 6(Right shoulder): When pushed, will increment the smoothing of the motion magic up to 8 * Left Joystick Y-Axis: * + Arcade Drive: Drive robot forward and reverse * + Motion Magic: Servo robot forward and reverse [-6, 6] rotations @@ -65,6 +67,7 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.SensorTerm; import com.ctre.phoenix.motorcontrol.StatusFrame; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -84,6 +87,8 @@ public class Robot extends TimedRobot { boolean _firstCall = false; boolean _state = false; + int _smoothing; + @Override public void robotInit() { /* Not used in this project */ @@ -180,6 +185,7 @@ public void teleopInit(){ /* Initialize */ _firstCall = true; _state = false; + _rightMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, 10); zeroSensors(); } @@ -200,6 +206,20 @@ public void teleopPeriodic() { else if (btns[1] && !_btns[1]) { zeroSensors(); // Zero sensors } + if(btns[5] && !_btns[5]) { + _smoothing--; // Decrement smoothing + if(_smoothing < 0) _smoothing = 0; // Cap smoothing + _rightMaster.configMotionSCurveStrength(_smoothing); + + System.out.println("Smoothing value is: " + _smoothing); + } + if(btns[6] && !_btns[6]) { + _smoothing++; // Increment smoothing + if(_smoothing > 8) _smoothing = 8; // Cap smoothing + _rightMaster.configMotionSCurveStrength(_smoothing); + + System.out.println("Smoothing value is: " + _smoothing); + } System.arraycopy(btns, 0, _btns, 0, Constants.kNumButtonsPlusOne); if(!_state){ diff --git a/Java/MotionMagic_AuxStraightPigeon/src/main/java/frc/robot/Robot.java b/Java/MotionMagic_AuxStraightPigeon/src/main/java/frc/robot/Robot.java index 71088930..6b3e76a3 100644 --- a/Java/MotionMagic_AuxStraightPigeon/src/main/java/frc/robot/Robot.java +++ b/Java/MotionMagic_AuxStraightPigeon/src/main/java/frc/robot/Robot.java @@ -42,6 +42,8 @@ * Button 2: When pressed, toggle between Arcade Drive and Motion Magic * When toggling into Motion Magic, the current heading is saved and used as the * auxiliary closed loop target. Can be changed by toggling out and in again. + * Button 5(Left shoulder): When pushed, will decrement the smoothing of the motion magic down to 0 + * Button 6(Right shoulder): When pushed, will increment the smoothing of the motion magic up to 8 * Left Joystick Y-Axis: * + Arcade Drive: Drive robot forward and reverse * + Motion Magic: Servo robot forward and reverse [-6, 6] rotations @@ -67,6 +69,7 @@ import com.ctre.phoenix.motorcontrol.SensorTerm; import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame; import com.ctre.phoenix.motorcontrol.StatusFrame; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FollowerType; import com.ctre.phoenix.motorcontrol.DemandType; @@ -90,6 +93,8 @@ public class Robot extends TimedRobot { boolean _state = false; double _targetAngle = 0; + int _smoothing; + @Override public void robotInit() { /* Not used in this project */ @@ -221,6 +226,7 @@ public void teleopInit(){ /* Initialize */ _firstCall = true; _state = false; + _rightMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, 10); zeroSensors(); } @@ -241,6 +247,20 @@ public void teleopPeriodic() { }else if (_currentBtns[1] && !_previous_currentBtns[1]) { zeroSensors(); // Zero Sensors } + if(_currentBtns[5] && !_previous_currentBtns[5]) { + _smoothing--; // Decrement smoothing + if(_smoothing < 0) _smoothing = 0; // Cap smoothing + _rightMaster.configMotionSCurveStrength(_smoothing); + + System.out.println("Smoothing value is: " + _smoothing); + } + if(_currentBtns[6] && !_previous_currentBtns[6]) { + _smoothing++; // Increment smoothing + if(_smoothing > 8) _smoothing = 8; // Cap smoothing + _rightMaster.configMotionSCurveStrength(_smoothing); + + System.out.println("Smoothing value is: " + _smoothing); + } System.arraycopy(_currentBtns, 0, _previous_currentBtns, 0, Constants.kNumButtonsPlusOne); if(!_state){ diff --git a/Java/MotionMagic_AuxStraightQuadrature/src/main/java/frc/robot/Robot.java b/Java/MotionMagic_AuxStraightQuadrature/src/main/java/frc/robot/Robot.java index e7a86e3c..8a0371c0 100644 --- a/Java/MotionMagic_AuxStraightQuadrature/src/main/java/frc/robot/Robot.java +++ b/Java/MotionMagic_AuxStraightQuadrature/src/main/java/frc/robot/Robot.java @@ -42,6 +42,8 @@ * Controls: * Button 1: When pressed, zero all sensors. Set quadrature encoders' positions to 0. * Button 2: When pressed, toggle between Arcade Drive and Motion Magic. + * Button 5(Left shoulder): When pushed, will decrement the smoothing of the motion magic down to 0 + * Button 6(Right shoulder): When pushed, will increment the smoothing of the motion magic up to 8 * When toggling into Motion Magic, the current heading is saved and used * as the the closed loop target. Can be changed by toggling out and in again. * Left Joystick Y-Axis: @@ -68,6 +70,7 @@ import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.SensorTerm; import com.ctre.phoenix.motorcontrol.StatusFrame; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FollowerType; import com.ctre.phoenix.motorcontrol.DemandType; @@ -90,6 +93,8 @@ public class Robot extends TimedRobot { double _lockedDistance = 0; double _targetAngle = 0; + int _smoothing; + @Override public void robotInit() { /* Not used in this project */ @@ -218,6 +223,7 @@ public void teleopInit(){ /* Initialize */ _firstCall = true; _state = false; + _rightMaster.setStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, 10); zeroSensors(); } @@ -239,6 +245,20 @@ public void teleopPeriodic() { }else if (btns[1] && !_btns[1]) { zeroSensors(); // Zero Sensors } + if(btns[5] && !_btns[5]) { + _smoothing--; // Decrement smoothing + if(_smoothing < 0) _smoothing = 0; // Cap smoothing + _rightMaster.configMotionSCurveStrength(_smoothing); + + System.out.println("Smoothing value is: " + _smoothing); + } + if(btns[6] && !_btns[6]) { + _smoothing++; // Increment smoothing + if(_smoothing > 8) _smoothing = 8; // Cap smoothing + _rightMaster.configMotionSCurveStrength(_smoothing); + + System.out.println("Smoothing value is: " + _smoothing); + } System.arraycopy(btns, 0, _btns, 0, Constants.kNumButtonsPlusOne); if(!_state){ diff --git a/Java/PigeonRemoteSensor/.wpilib/wpilib_preferences.json b/Java/PigeonRemoteSensor/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..7486464e --- /dev/null +++ b/Java/PigeonRemoteSensor/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2019", + "teamNumber": 3539 +} \ No newline at end of file diff --git a/Java/PigeonRemoteSensor/build.gradle b/Java/PigeonRemoteSensor/build.gradle new file mode 100644 index 00000000..9128ffce --- /dev/null +++ b/Java/PigeonRemoteSensor/build.gradle @@ -0,0 +1,61 @@ +plugins { + id "java" + id "edu.wpi.first.GradleRIO" version "2019.1.1" +} + +def ROBOT_MAIN_CLASS = "frc.robot.Main" + +// Define my targets (RoboRIO) and artifacts (deployable files) +// This is added by GradleRIO's backing project EmbeddedTools. +deploy { + targets { + roboRIO("roborio") { + // Team number is loaded either from the .wpilib/wpilib_preferences.json + // or from command line. If not found an exception will be thrown. + // You can use getTeamOrDefault(team) instead of getTeamNumber if you + // want to store a team number in this file. + team = frc.getTeamNumber() + } + } + artifacts { + frcJavaArtifact('frcJava') { + targets << "roborio" + // Debug can be overridden by command line, for use with VSCode + debug = frc.getDebugOrDefault(false) + } + // Built in artifact to deploy arbitrary files to the roboRIO. + fileTreeArtifact('frcStaticFileDeploy') { + // The directory below is the local directory to deploy + files = fileTree(dir: 'src/main/deploy') + // Deploy to RoboRIO target, into /home/lvuser/deploy + targets << "roborio" + directory = '/home/lvuser/deploy' + } + } +} + +// Set this to true to enable desktop support. +def includeDesktopSupport = false + +// Maven central needed for JUnit +repositories { + mavenCentral() +} + +// Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. +// Also defines JUnit 4. +dependencies { + compile wpi.deps.wpilib() + compile wpi.deps.vendor.java() + nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio) + nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop) + testCompile 'junit:junit:4.12' +} + +// Setting up my Jar File. In this case, adding all libraries into the main jar ('fat jar') +// in order to make them all available at runtime. Also adding the manifest so WPILib +// knows where to look for our Robot Class. +jar { + from { configurations.compile.collect { it.isDirectory() ? it : zipTree(it) } } + manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) +} diff --git a/Java/PigeonRemoteSensor/gradle/wrapper/gradle-wrapper.jar b/Java/PigeonRemoteSensor/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 00000000..457aad0d Binary files /dev/null and b/Java/PigeonRemoteSensor/gradle/wrapper/gradle-wrapper.jar differ diff --git a/Java/PigeonRemoteSensor/gradle/wrapper/gradle-wrapper.properties b/Java/PigeonRemoteSensor/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..d08253ca --- /dev/null +++ b/Java/PigeonRemoteSensor/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/Java/PigeonRemoteSensor/gradlew b/Java/PigeonRemoteSensor/gradlew new file mode 100644 index 00000000..af6708ff --- /dev/null +++ b/Java/PigeonRemoteSensor/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/Java/PigeonRemoteSensor/gradlew.bat b/Java/PigeonRemoteSensor/gradlew.bat new file mode 100644 index 00000000..6d57edc7 --- /dev/null +++ b/Java/PigeonRemoteSensor/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/Java/PigeonRemoteSensor/settings.gradle b/Java/PigeonRemoteSensor/settings.gradle new file mode 100644 index 00000000..b0f4d48a --- /dev/null +++ b/Java/PigeonRemoteSensor/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/Java/PigeonRemoteSensor/src/main/java/frc/robot/Instrum.java b/Java/PigeonRemoteSensor/src/main/java/frc/robot/Instrum.java new file mode 100644 index 00000000..9a65e1aa --- /dev/null +++ b/Java/PigeonRemoteSensor/src/main/java/frc/robot/Instrum.java @@ -0,0 +1,165 @@ +/** + * Instrumentation Class for printing + */ +package frc.robot; + +import com.ctre.phoenix.motorcontrol.can.BaseMotorController; +import com.ctre.phoenix.sensors.PigeonIMU; + +public class Instrum { + /* Tracking variables for instrumentation */ + static int printCounter = 0; + + public static void Process(PigeonIMU pidgey, BaseMotorController rightSideTalon, boolean printEnable, int signalSelection, int axisSelection) { + + /* track loops since last print */ + if (printCounter < 0xFFFF) { + ++printCounter; + } + + /* if printing is off, leave immedietely */ + if (printEnable == false) + return; + + if (printCounter < 50) { + /* not enough time since last print */ + } else { + /* print the selected signals */ + switch (signalSelection) { + case 0: + /** Printing YPR Values with Talon */ + double[] ypr = new double[3]; + pidgey.getYawPitchRoll(ypr); + + System.out.println("Printing Yaw/Pitch/Roll"); + System.out.println("Pigeon value: " + ypr[axisSelection]); + if (rightSideTalon != null) + System.out.println("Talon Selected Sensor Value: " + rightSideTalon.getSelectedSensorPosition()); + else + System.out.println("No Talon setup for this platform"); + System.out.println(); + break; + case 1: + /** Printing Quaternions */ + double[] quaternions = new double[4]; + pidgey.get6dQuaternion(quaternions); + System.out.println("Printing Quaternion Values"); + System.out.println("W: " + quaternions[0]); + System.out.println("X: " + quaternions[1]); + System.out.println("Y: " + quaternions[2]); + System.out.println("Z: " + quaternions[3]); + System.out.println(); + break; + case 2: + /** Printing Accumulated Gyro */ + double[] accumGyro = new double[3]; + pidgey.getAccumGyro(accumGyro); + System.out.println("X: " + accumGyro[0]); + System.out.println("Y: " + accumGyro[1]); + System.out.println("Z: " + accumGyro[2]); + System.out.println(); + break; + case 3: + /** Printing Biased Accelerometer Angles */ + short[] biasedAccel = new short[3]; + pidgey.getBiasedAccelerometer(biasedAccel); + System.out.println("X: " + biasedAccel[0]); + System.out.println("Y: " + biasedAccel[1]); + System.out.println("Z: " + biasedAccel[2]); + System.out.println(); + break; + case 4: + /** Printing Raw Gyro */ + double[] rawGyro = new double[3]; + pidgey.getRawGyro(rawGyro); + System.out.println("X: " + rawGyro[0]); + System.out.println("Y: " + rawGyro[1]); + System.out.println("Z: " + rawGyro[2]); + System.out.println(); + break; + case 5: + /** Printing Accelerometer Angles */ + double[] accelAngles = new double[3]; + pidgey.getAccelerometerAngles(accelAngles); + System.out.println("X: " + accelAngles[0]); + System.out.println("Y: " + accelAngles[1]); + System.out.println("Z: " + accelAngles[2]); + System.out.println(); + break; + case 6: + /** Printing Biased Magnetometer Angles */ + short[] biasedMagnet = new short[3]; + pidgey.getBiasedMagnetometer(biasedMagnet); + System.out.println("X: " + biasedMagnet[0]); + System.out.println("Y: " + biasedMagnet[1]); + System.out.println("Z: " + biasedMagnet[2]); + System.out.println(); + break; + case 7: + /** Printing Raw Magnetometer Angles */ + short[] rawMagnet = new short[3]; + pidgey.getRawMagnetometer(rawMagnet); + System.out.println("X: " + rawMagnet[0]); + System.out.println("Y: " + rawMagnet[1]); + System.out.println("Z: " + rawMagnet[2]); + System.out.println(); + break; + } + } + } + + public static void PrintSelection(int pigeonInformationUsed) { + /** Toggle using ypr or quaternions/accum signals */ + switch (pigeonInformationUsed) { + case 0: + /** Use YPR */ + System.out.println("============================"); + System.out.println("Reading YPR from Pigeon"); + System.out.println("============================"); + break; + case 1: + /** Use Quaternion */ + System.out.println("============================"); + System.out.println("Reading Quaternion from Pigeon"); + System.out.println("============================"); + break; + case 2: + /** Use Accum Gyro */ + System.out.println("============================"); + System.out.println("Reading Accum Gyro from Pigeon"); + System.out.println("============================"); + break; + case 3: + /** Use Biased Accel */ + System.out.println("============================"); + System.out.println("Reading Biased Accel from Pigeon"); + System.out.println("============================"); + break; + case 4: + /** Use Raw Gyro */ + System.out.println("============================"); + System.out.println("Reading Raw Gyro from Pigeon"); + System.out.println("============================"); + break; + case 5: + /** Use Accelerometer Angles */ + System.out.println("============================"); + System.out.println("Reading Accelerometer from Pigeon"); + System.out.println("============================"); + break; + case 6: + /** Use Biased Magnetometer */ + System.out.println("============================"); + System.out.println("Reading Biased Magnetometer from Pigeon"); + System.out.println("============================"); + break; + case 7: + /** Use Raw Magnetometer */ + System.out.println("============================"); + System.out.println("Reading Raw Magnetometer from Pigeon"); + System.out.println("============================"); + break; + } + } + +} diff --git a/Java/PigeonRemoteSensor/src/main/java/frc/robot/Main.java b/Java/PigeonRemoteSensor/src/main/java/frc/robot/Main.java new file mode 100644 index 00000000..5b3238ad --- /dev/null +++ b/Java/PigeonRemoteSensor/src/main/java/frc/robot/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/Java/PigeonRemoteSensor/src/main/java/frc/robot/Robot.java b/Java/PigeonRemoteSensor/src/main/java/frc/robot/Robot.java new file mode 100644 index 00000000..26729b36 --- /dev/null +++ b/Java/PigeonRemoteSensor/src/main/java/frc/robot/Robot.java @@ -0,0 +1,277 @@ +/** + * Phoenix Software License Agreement + * + * Copyright (C) Cross The Road Electronics. All rights + * reserved. + * + * Cross The Road Electronics (CTRE) licenses to you the right to + * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and + * Phoenix Software API Libraries ONLY when in use with CTR Electronics hardware products + * as well as the FRC roboRIO when in use in FRC Competition. + * + * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT + * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT + * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL + * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, + * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF + * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS + * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE + * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER + * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT + * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE + */ + +/** + * PLEASE READ FIRST + * + * Description: + * The PigeonRemoteSensor example demonstrates using a PigeonIMU as a remote sensor + * Tested with Logitech F350 USB Gamepad inserted into Driver Station + * + * Be sure to configure the correct motor controller and pigeon + * The example has all the tools needed to determine if the pigeon is in a good + * position on the robot i.e. it will measure the desired values under all + * circumstances the robot will experience. + * + * The first thing you should do is determine if the placement of the pigeon is + * close enough to the center of rotation - The Pigeon IMU is a sensor that is + * impacted by excessive and sustained g-forces. Keeping it near the center of + * rotation will prevent this excessive and sustained g-force while the robot is + * rotating. The procedure for this is as follows: 1. Drive the robot into an + * immovable flat obstacle, such as a wall 2. Zero the yaw and accumZ by + * pressing the A button 3. Drive the robot in a zero turn at max speed for + * about 30 seconds 4. Drive the robot in the opposite direction for about 30 + * seconds or until yaw is around 0 5. Drive back up to the immovable object, + * and measure the yaw value and accum Z value 6. If yaw is incorrect and accumZ + * is correct, move the IMU closer to Centor of rotation and repeat + * + * The second thing you should do is temperature-calibrate the pigeon - Follow + * the guide in the documentation Bring-Up Pigeon for this + * https://phoenix-documentation.readthedocs.io/en/latest/ch11_BringUpPigeon.html#temperature-calibration + * + * A quick guide on how to temperature calibrate is below: 1. Ensure pigeon is + * cool before beginning temperature calibration. This can be confirmed with a + * self test 2. Enter temperature calibration mode. This is done either using + * the API or using Phoenix Tuner 3. Heat the pigeon. 4. Once the pigeon has + * seen a sufficient range of temperatures, it will momentarily blink green, + * then cleanly boot-calibrate. + * + * Controls: + * X, B Buttons: Cycle between yaw, pitch, and roll as the selected filter for the motor controller + * A Button: Zero yaw + * Y Button: Start printing information every 50 loops + * Left, Right Bumpers: Cycle between printing YPR information, and any other information pigeon has + * Left joystick up/down: Throttle for the Robot + * Right joystick left/right: Turn for the Robot + */ + +package frc.robot; + +import edu.wpi.first.wpilibj.*; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.RemoteFeedbackDevice; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; +import com.ctre.phoenix.motorcontrol.can.*; +import com.ctre.phoenix.sensors.PigeonIMU; + +public class Robot extends TimedRobot { + + /** pigeon instance, this is instantiated later, leave this alone */ + PigeonIMU _pidgey; + + /* pick one of these */ + TalonSRX _pigeonTalon = new TalonSRX(2); /* Pigeon is ribbon cabled to this Talon */ + // TalonSRX _pigeonTalon = null; /* Pigeon is on CAN bus, so there is no Talon + + /** + * if Pigeon is on CAN-bus, enter the device ID. its connected to Talon, this + * does not matter + */ + final int kPigeonID = 1; + + /** + * If using remote talon features for drive train, provide the drivetrain + * talons/victors. Otherwise set them to null. Create both or null both. + * + * The RIGHT SIDE Talon is used to capture Pigeon values for Arc'ing / Differential-closed-looping. + * + */ + // BaseMotorController leftSide = new TalonSRX(1); // using Talon for drive train + // BaseMotorController rightSide = new VictorSPX(2); // using Talon for drive train + BaseMotorController _leftSide = null; // no talons, just polling Pigeon for direct processing + BaseMotorController _rghtSide = null; // no talons, just polling Pigeon for direct processing + + /** + * Which filter to use in Talon to capture Pigeon (0 or 1). If you are not using + * Pigeon in your drietrain closed-loop, this doesn't matter. + */ + final int kRemoteFilter = 0; + + /* joystick for control */ + Joystick _joy = new Joystick(0); + + int _axisSelection = 0; //!< [0,2] => [Yaw,Pitch,Roll] + int _signalSelection = 0; //!< [0,7] => What signal to print, see Instrum implem + boolean _printEnable = false; //!< True => print signal values periodically + + /* timeouts for certain blocking actions */ + final int kTimeoutMs = 50; + + @Override + public void robotInit() { + /* create the pigeon */ + if (IsPigeonOnCanbus()) + _pidgey = new PigeonIMU(kPigeonID); + else + _pidgey = new PigeonIMU(_pigeonTalon); + + /* + * if using the pigeon in your Talon closed loop, setup talon to capture Pigeon + * data + */ + if (_rghtSide != null) { + /* factory default configs */ + _leftSide.configFactoryDefault(); + _rghtSide.configFactoryDefault(); + + /** Configure filter to use Yaw for the moment */ + if (kRemoteFilter == 0) + _rghtSide.configSelectedFeedbackSensor(RemoteFeedbackDevice.RemoteSensor0); + else + _rghtSide.configSelectedFeedbackSensor(RemoteFeedbackDevice.RemoteSensor1); + + /* config the Talon/Vic's remote sensor */ + SetupTalonRemoteSensorSource(); + + /* pick the inverts, pushing stick forward should cause + green LEDs on all MCs, forward robot motion */ + _leftSide.setInverted(false); // <<<<<<<<<< adjust this + _rghtSide.setInverted(true); // <<<<<<<<<< adjust this + } + } + + /** + * helper routine to track if pigeon is on CAN or ribbon cable, this influces + * how to setup devices + */ + boolean IsPigeonOnCanbus() { + if (_pigeonTalon == null) + return true; + return false; + } + + void SetupTalonRemoteSensorSource() { + if (_rghtSide == null) { + System.out.println("============================"); + System.out.println("Motor controller not selected, so remote filter is not setup."); + System.out.println("============================"); + System.out.println(); + + } else { + + RemoteSensorSource senSource = RemoteSensorSource.Pigeon_Yaw; + String msg = ""; + + switch (_axisSelection) { + case 0: + if (IsPigeonOnCanbus()) + senSource = RemoteSensorSource.Pigeon_Yaw; + else + senSource = RemoteSensorSource.GadgeteerPigeon_Yaw; + break; + case 1: + if (IsPigeonOnCanbus()) + senSource = RemoteSensorSource.Pigeon_Pitch; + else + senSource = RemoteSensorSource.GadgeteerPigeon_Pitch; + break; + case 2: + if (IsPigeonOnCanbus()) + senSource = RemoteSensorSource.Pigeon_Roll; + else + senSource = RemoteSensorSource.GadgeteerPigeon_Roll; + break; + } + _rghtSide.configRemoteFeedbackFilter(_pidgey.getDeviceID(), senSource, kRemoteFilter); + + System.out.println("============================"); + System.out.println("Motor controller filtering for " + senSource.toString()); + System.out.println("============================"); + System.out.println(); + } + } + + @Override + public void robotPeriodic() { + + /* button 2 will zero Pigeon */ + if (_joy.getRawButtonPressed(2)) { + /** Zero yaw, this has to be done using the pigeon, not the motor controller */ + _pidgey.setYaw(0, kTimeoutMs); + _pidgey.setAccumZAngle(0, kTimeoutMs); + + System.out.println("============================"); + System.out.println("Yaw and accumulated Z zero'ed"); + System.out.println("============================"); + System.out.println(); + } + + /* button 1 and 3 will change which Pigeon axis to instrument */ + if (_joy.getRawButtonPressed(1) || _joy.getRawButtonPressed(3)) { + if (_joy.getRawButton(1)) + _axisSelection--; + else + _axisSelection++; + /* some clever math to wrap 2=>0, and 0=>2 */ + _axisSelection = (_axisSelection + 3) % 3; + + /** Increment feedback filter to use other degree from pigeon */ + SetupTalonRemoteSensorSource(); + } + + /* button 4 will toggle the printing. */ + if (_joy.getRawButtonPressed(4)) { + /** Toggle printing information */ + _printEnable = !_printEnable; + System.out.println("============================"); + System.out.println("Printing is " + (_printEnable ? "Enabled" : "Disabled")); + System.out.println("============================"); + System.out.println(); + } + + /* Shoulder buttons 5 and 6 will change the signal selection */ + if (_joy.getRawButtonPressed(5) || _joy.getRawButtonPressed(6)) { + + if (_joy.getRawButton(5)) + _signalSelection--; + else + _signalSelection++; + + /* some clever math to wrap 8=>0, and 8=>2 */ + _signalSelection = (_signalSelection + 8) % 8; + + /* print the selection change */ + Instrum.PrintSelection(_signalSelection); + } + + /* print latest signal values (if printing enabled) */ + Instrum.Process(_pidgey, _rghtSide, _printEnable, _signalSelection, _axisSelection); + } + + @Override + public void teleopPeriodic() { + /* get joystick values */ + double frwd = +1 * _joy.getRawAxis(1); /* signed so positive means drive forward (green LEDs) */ + double turn = -1 * _joy.getRawAxis(2); /* signed so positive means turn to the left */ + + if (_rghtSide == null) { + /* there are no talons in this setup */ + } else { + /* drive the drivetrain assuming a simple diff drive */ + _leftSide.set(ControlMode.PercentOutput, frwd, DemandType.ArbitraryFeedForward, -turn); + _rghtSide.set(ControlMode.PercentOutput, frwd, DemandType.ArbitraryFeedForward, +turn); + } + } +} diff --git a/Java/PigeonRemoteSensor/vendordeps/Phoenix.json b/Java/PigeonRemoteSensor/vendordeps/Phoenix.json new file mode 100644 index 00000000..a1654ec4 --- /dev/null +++ b/Java/PigeonRemoteSensor/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