Squashed 'third_party/allwpilib_2019/' content from commit bd05dfa1c
Change-Id: I2b1c2250cdb9b055133780c33593292098c375b7
git-subtree-dir: third_party/allwpilib_2019
git-subtree-split: bd05dfa1c7cca74c4fac451e7b9d6a37e7b53447
diff --git a/wpilibcExamples/.styleguide b/wpilibcExamples/.styleguide
new file mode 100644
index 0000000..d39dee7
--- /dev/null
+++ b/wpilibcExamples/.styleguide
@@ -0,0 +1,22 @@
+cppHeaderFileInclude {
+ \.h$
+ \.hpp$
+ \.inc$
+}
+
+cppSrcFileInclude {
+ \.cpp$
+}
+
+includeOtherLibs {
+ ^HAL/
+ ^cameraserver/
+ ^cscore
+ ^frc/
+ ^networktables/
+ ^ntcore
+ ^opencv2/
+ ^support/
+ ^vision/
+ ^wpi/
+}
diff --git a/wpilibcExamples/build.gradle b/wpilibcExamples/build.gradle
new file mode 100644
index 0000000..3c87203
--- /dev/null
+++ b/wpilibcExamples/build.gradle
@@ -0,0 +1,233 @@
+import org.gradle.language.base.internal.ProjectLayout
+
+apply plugin: 'cpp'
+apply plugin: 'c'
+apply plugin: 'visual-studio'
+apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: ExtraTasks
+
+apply from: '../shared/config.gradle'
+
+ext.examplesMap = [:]
+ext.templatesMap = [:]
+
+File examplesTree = file("$projectDir/src/main/cpp/examples")
+examplesTree.list(new FilenameFilter() {
+ @Override
+ public boolean accept(File current, String name) {
+ return new File(current, name).isDirectory();
+ }
+}).each {
+ examplesMap.put(it, [])
+}
+File templatesTree = file("$projectDir/src/main/cpp/templates")
+templatesTree.list(new FilenameFilter() {
+ @Override
+ public boolean accept(File current, String name) {
+ return new File(current, name).isDirectory();
+ }
+}).each {
+ templatesMap.put(it, [])
+}
+
+
+ext {
+ sharedCvConfigs = examplesMap + templatesMap + [commands: []]
+ staticCvConfigs = [:]
+ useJava = false
+ useCpp = true
+}
+
+apply from: "${rootDir}/shared/opencv.gradle"
+
+ext {
+ chipObjectComponents = ['commands']
+ netCommComponents = ['commands']
+ examplesMap.each { key, value ->
+ chipObjectComponents << key.toString()
+ netCommComponents << key.toString()
+ }
+ templatesMap.each { key, value ->
+ chipObjectComponents << key.toString()
+ netCommComponents << key.toString()
+ }
+}
+
+apply from: "${rootDir}/shared/nilibraries.gradle"
+
+model {
+ components {
+ commands(NativeLibrarySpec) {
+ binaries.all { binary ->
+ if (binary in StaticLibraryBinarySpec) {
+ binary.buildable = false
+ return
+ }
+ lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+ lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ project(':hal').addHalDependency(binary, 'shared')
+ lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ }
+ sources {
+ cpp {
+ source {
+ srcDirs = ['src/main/cpp/commands']
+ include '**/*.cpp'
+ }
+ exportedHeaders {
+ srcDirs 'src/main/cpp/commands'
+ include '**/*.h'
+ }
+ }
+ }
+ }
+
+ examplesMap.each { key, value ->
+ "${key}"(NativeExecutableSpec) {
+ targetBuildTypes 'debug'
+ binaries.all { binary ->
+ lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+ lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ project(':hal').addHalDependency(binary, 'shared')
+ lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ if (binary.targetPlatform.architecture.name != 'athena') {
+ lib project: ':simulation:halsim_lowfi', library: 'halsim_lowfi', linkage: 'shared'
+ lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
+ lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
+ lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
+ }
+ }
+ sources {
+ cpp {
+ source {
+ srcDirs 'src/main/cpp/examples/' + "${key}" + "/cpp"
+ include '**/*.cpp'
+ }
+ exportedHeaders {
+ srcDirs 'src/main/cpp/examples/' + "${key}" + "/include"
+ include '**/*.h'
+ }
+ }
+ }
+ sources {
+ c {
+ source {
+ srcDirs 'src/main/cpp/examples/' + "${key}" + "/c"
+ include '**/*.c'
+ }
+ exportedHeaders {
+ srcDirs 'src/main/cpp/examples/' + "${key}" + "/include"
+ include '**/*.h'
+ }
+ }
+ }
+ }
+ }
+ templatesMap.each { key, value ->
+ "${key}"(NativeExecutableSpec) {
+ targetBuildTypes 'debug'
+ binaries.all { binary ->
+ lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+ lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+ lib project: ':cscore', library: 'cscore', linkage: 'shared'
+ project(':hal').addHalDependency(binary, 'shared')
+ lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+ lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+ binary.tasks.withType(CppCompile) {
+ if (!(binary.toolChain in VisualCpp)) {
+ cppCompiler.args "-Wno-error=deprecated-declarations"
+ }
+ }
+ if (binary.targetPlatform.architecture.name != 'athena') {
+ lib project: ':simulation:halsim_lowfi', library: 'halsim_lowfi', linkage: 'shared'
+ lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
+ lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
+ lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
+ }
+ }
+ sources {
+ cpp {
+ source {
+ srcDirs 'src/main/cpp/templates/' + "${key}" + "/cpp"
+ include '**/*.cpp'
+ }
+ exportedHeaders {
+ srcDirs 'src/main/cpp/templates/' + "${key}" + "/include"
+ include '**/*.h'
+ }
+ }
+ }
+ }
+ }
+ }
+ tasks {
+ def b = $.binaries
+ b.each { binary->
+ if (binary in NativeExecutableBinarySpec) {
+ def installDir = binary.tasks.install.installDirectory.get().toString() + File.separatorChar
+ def runFile = binary.tasks.install.runScriptFile.get().asFile.toString()
+
+ binary.tasks.install.doLast {
+ if (binary.targetPlatform.operatingSystem.isWindows()) {
+ // Windows batch scripts
+ def fileName = binary.component.name + 'LowFi.bat'
+ def file = new File(installDir + fileName)
+ file.withWriter { out ->
+ out.println '@ECHO OFF'
+ out.print 'SET HALSIM_EXTENSIONS='
+ out.print '"' + new File(installDir + 'lib\\halsim_lowfi.dll').toString() + '";'
+ out.println '"' + new File(installDir + 'lib\\halsim_ds_nt.dll').toString() + '"'
+ out.println runFile + ' %*'
+ }
+
+ fileName = binary.component.name + 'LowFiRealDS.bat'
+ file = new File(installDir + fileName)
+ file.withWriter { out ->
+ out.println '@ECHO OFF'
+ out.print 'SET HALSIM_EXTENSIONS='
+ out.print '"' + new File(installDir + 'lib\\halsim_lowfi.dll').toString() + '";'
+ out.println '"' + new File(installDir + 'lib\\halsim_ds_socket.dll').toString() + '"'
+ out.println runFile + ' %*'
+ }
+ } else {
+ def fileName = binary.component.name + 'LowFi.sh'
+ def file = new File(installDir + fileName)
+
+ file.withWriter { out ->
+ out.print 'export HALSIM_EXTENSIONS='
+ out.print '"' + new File(installDir + '/lib/libhalsim_lowfi.so').toString() + '";'
+ out.println '"' + new File(installDir + '/lib/libhalsim_ds_nt.so').toString() + '"'
+ out.println runFile + ' "$@"'
+ }
+
+ fileName = binary.component.name + 'LowFiRealDS.sh'
+ file = new File(installDir + fileName)
+ file.withWriter { out ->
+ out.print 'export HALSIM_EXTENSIONS='
+ out.print '"' + new File(installDir + '/lib/libhalsim_lowfi.so').toString() + '":'
+ out.println '"' + new File(installDir + '/lib/libhalsim_ds_socket.so').toString() + '"'
+ out.println runFile + ' "$@"'
+ }
+ }
+ }
+
+ }
+ }
+ }
+}
+apply from: 'publish.gradle'
+
+ext {
+ templateDirectory = new File("$projectDir/src/main/cpp/templates/")
+ templateFile = new File("$projectDir/src/main/cpp/templates/templates.json")
+ exampleDirectory = new File("$projectDir/src/main/cpp/examples/")
+ exampleFile = new File("$projectDir/src/main/cpp/examples/examples.json")
+ commandDirectory = new File("$projectDir/src/main/cpp/commands/")
+ commandFile = new File("$projectDir/src/main/cpp/commands/commands.json")
+}
+
+apply from: "${rootDir}/shared/examplecheck.gradle"
diff --git a/wpilibcExamples/publish.gradle b/wpilibcExamples/publish.gradle
new file mode 100644
index 0000000..ec645ae
--- /dev/null
+++ b/wpilibcExamples/publish.gradle
@@ -0,0 +1,94 @@
+apply plugin: 'maven-publish'
+
+def pubVersion
+if (project.hasProperty("publishVersion")) {
+ pubVersion = project.publishVersion
+} else {
+ pubVersion = WPILibVersion.version
+}
+
+def baseExamplesArtifactId = 'examples'
+def baseTemplatesArtifactId = 'templates'
+def baseCommandsArtifactId = 'commands'
+def artifactGroupId = 'edu.wpi.first.wpilibc'
+
+def examplesZipBaseName = '_GROUP_edu_wpi_first_wpilibc_ID_examples_CLS'
+def templatesZipBaseName = '_GROUP_edu_wpi_first_wpilibc_ID_templates_CLS'
+def commandsZipBaseName = '_GROUP_edu_wpi_first_wpilibc_ID_commands_CLS'
+
+def outputsFolder = file("$project.buildDir/outputs")
+
+task cppExamplesZip(type: Zip) {
+ destinationDir = outputsFolder
+ baseName = examplesZipBaseName
+
+ from(licenseFile) {
+ into '/'
+ }
+
+ from('src/main/cpp/examples') {
+ into 'examples'
+ }
+}
+
+task cppTemplatesZip(type: Zip) {
+ destinationDir = outputsFolder
+ baseName = templatesZipBaseName
+
+ from(licenseFile) {
+ into '/'
+ }
+
+ from('src/main/cpp/templates') {
+ into 'templates'
+ }
+}
+
+task cppCommandsZip(type: Zip) {
+ destinationDir = outputsFolder
+ baseName = commandsZipBaseName
+
+ from(licenseFile) {
+ into '/'
+ }
+
+ from('src/main/cpp/commands') {
+ into 'commands'
+ }
+}
+
+build.dependsOn cppTemplatesZip
+build.dependsOn cppExamplesZip
+build.dependsOn cppCommandsZip
+
+addTaskToCopyAllOutputs(cppTemplatesZip)
+addTaskToCopyAllOutputs(cppExamplesZip)
+addTaskToCopyAllOutputs(cppCommandsZip)
+
+publishing {
+ publications {
+ examples(MavenPublication) {
+ artifact cppExamplesZip
+
+ artifactId = baseExamplesArtifactId
+ groupId artifactGroupId
+ version pubVersion
+ }
+
+ templates(MavenPublication) {
+ artifact cppTemplatesZip
+
+ artifactId = baseTemplatesArtifactId
+ groupId artifactGroupId
+ version pubVersion
+ }
+
+ commands(MavenPublication) {
+ artifact cppCommandsZip
+
+ artifactId = baseCommandsArtifactId
+ groupId artifactGroupId
+ version pubVersion
+ }
+ }
+}
diff --git a/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.cpp b/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.cpp
new file mode 100644
index 0000000..07c233e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.cpp
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "ReplaceMeCommand.h"
+
+ReplaceMeCommand::ReplaceMeCommand() {
+ // Use Requires() here to declare subsystem dependencies
+ // eg. Requires(Robot::chassis.get());
+}
+
+// Called just before this Command runs the first time
+void ReplaceMeCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void ReplaceMeCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool ReplaceMeCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void ReplaceMeCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void ReplaceMeCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.h b/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.h
new file mode 100644
index 0000000..49a3139
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+class ReplaceMeCommand : public frc::Command {
+ public:
+ ReplaceMeCommand();
+ void Initialize() override;
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+ void Interrupted() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.cpp b/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.cpp
new file mode 100644
index 0000000..1a431f4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "ReplaceMeCommandGroup.h"
+
+ReplaceMeCommandGroup::ReplaceMeCommandGroup() {
+ // Add Commands here:
+ // e.g. AddSequential(new Command1());
+ // AddSequential(new Command2());
+ // these will run in order.
+
+ // To run multiple commands at the same time,
+ // use AddParallel()
+ // e.g. AddParallel(new Command1());
+ // AddSequential(new Command2());
+ // Command1 and Command2 will run in parallel.
+
+ // A command group will require all of the subsystems that each member
+ // would require.
+ // e.g. if Command1 requires chassis, and Command2 requires arm,
+ // a CommandGroup containing them would require both the chassis and the
+ // arm.
+}
diff --git a/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.h b/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.h
new file mode 100644
index 0000000..a58f586
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.h
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+class ReplaceMeCommandGroup : public frc::CommandGroup {
+ public:
+ ReplaceMeCommandGroup();
+};
diff --git a/wpilibcExamples/src/main/cpp/commands/commands.json b/wpilibcExamples/src/main/cpp/commands/commands.json
new file mode 100644
index 0000000..bb8cbb6
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/commands.json
@@ -0,0 +1,123 @@
+[
+ {
+ "name": "Empty Class",
+ "description": "Create an empty command",
+ "tags": [
+ "class"
+ ],
+ "foldername": "emptyclass",
+ "headers": [
+ "ReplaceMeEmptyClass.h"
+ ],
+ "source": [
+ "ReplaceMeEmptyClass.cpp"
+ ],
+ "replacename": "ReplaceMeEmptyClass"
+ },
+ {
+ "name": "Command",
+ "description": "Create a base command",
+ "tags": [
+ "command"
+ ],
+ "foldername": "command",
+ "headers": [
+ "ReplaceMeCommand.h"
+ ],
+ "source": [
+ "ReplaceMeCommand.cpp"
+ ],
+ "replacename": "ReplaceMeCommand"
+ },
+ {
+ "name": "Command Group",
+ "description": "Create a command group",
+ "tags": [
+ "commandgroup"
+ ],
+ "foldername": "commandgroup",
+ "headers": [
+ "ReplaceMeCommandGroup.h"
+ ],
+ "source": [
+ "ReplaceMeCommandGroup.cpp"
+ ],
+ "replacename": "ReplaceMeCommandGroup"
+ },
+ {
+ "name": "Instant Command",
+ "description": "A command that runs immediately",
+ "tags": [
+ "instantcommand"
+ ],
+ "foldername": "instant",
+ "headers": [
+ "ReplaceMeInstantCommand.h"
+ ],
+ "source": [
+ "ReplaceMeInstantCommand.cpp"
+ ],
+ "replacename": "ReplaceMeInstantCommand"
+ },
+ {
+ "name": "Subsystem",
+ "description": "A subsystem",
+ "tags": [
+ "subsystem"
+ ],
+ "foldername": "subsystem",
+ "headers": [
+ "ReplaceMeSubsystem.h"
+ ],
+ "source": [
+ "ReplaceMeSubsystem.cpp"
+ ],
+ "replacename": "ReplaceMeSubsystem"
+ },
+ {
+ "name": "PID Subsystem",
+ "description": "A subsystem that runs a PID loop",
+ "tags": [
+ "pidsubsystem",
+ "pid"
+ ],
+ "foldername": "pidsubsystem",
+ "headers": [
+ "ReplaceMePIDSubsystem.h"
+ ],
+ "source": [
+ "ReplaceMePIDSubsystem.cpp"
+ ],
+ "replacename": "ReplaceMePIDSubsystem"
+ },
+ {
+ "name": "Timed Command",
+ "description": "A command that runs for a specified time",
+ "tags": [
+ "timedcommand"
+ ],
+ "foldername": "timed",
+ "headers": [
+ "ReplaceMeTimedCommand.h"
+ ],
+ "source": [
+ "ReplaceMeTimedCommand.cpp"
+ ],
+ "replacename": "ReplaceMeTimedCommand"
+ },
+ {
+ "name": "Trigger",
+ "description": "A command that runs off of a trigger",
+ "tags": [
+ "trigger"
+ ],
+ "foldername": "trigger",
+ "headers": [
+ "ReplaceMeTrigger.h"
+ ],
+ "source": [
+ "ReplaceMeTrigger.cpp"
+ ],
+ "replacename": "ReplaceMeTrigger"
+ }
+]
diff --git a/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.cpp b/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.cpp
new file mode 100644
index 0000000..4ff8ad8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.cpp
@@ -0,0 +1,10 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "ReplaceMeEmptyClass.h"
+
+ReplaceMeEmptyClass::ReplaceMeEmptyClass() {}
diff --git a/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.h b/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.h
new file mode 100644
index 0000000..131edab
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.h
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* 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
+
+class ReplaceMeEmptyClass {
+ public:
+ ReplaceMeEmptyClass();
+};
diff --git a/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.cpp b/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.cpp
new file mode 100644
index 0000000..c130e09
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.cpp
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "ReplaceMeInstantCommand.h"
+
+ReplaceMeInstantCommand::ReplaceMeInstantCommand() {
+ // Use Requires() here to declare subsystem dependencies
+ // eg. Requires(Robot::chassis.get());
+}
+
+// Called once when the command executes
+void ReplaceMeInstantCommand::Initialize() {}
diff --git a/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.h b/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.h
new file mode 100644
index 0000000..b50b617
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.h
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/InstantCommand.h>
+
+class ReplaceMeInstantCommand : public frc::InstantCommand {
+ public:
+ ReplaceMeInstantCommand();
+ void Initialize() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.cpp b/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.cpp
new file mode 100644
index 0000000..f446f8e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "ReplaceMePIDSubsystem.h"
+
+#include <frc/livewindow/LiveWindow.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+ReplaceMePIDSubsystem::ReplaceMePIDSubsystem()
+ : PIDSubsystem("ReplaceMePIDSubsystem", 1.0, 0.0, 0.0) {
+ // Use these to get going:
+ // SetSetpoint() - Sets where the PID controller should move the system
+ // to
+ // Enable() - Enables the PID controller.
+}
+
+double ReplaceMePIDSubsystem::ReturnPIDInput() {
+ // Return your input value for the PID loop
+ // e.g. a sensor, like a potentiometer:
+ // yourPot->SetAverageVoltage() / kYourMaxVoltage;
+ return 0;
+}
+
+void ReplaceMePIDSubsystem::UsePIDOutput(double output) {
+ // Use output to drive your system, like a motor
+ // e.g. yourMotor->Set(output);
+}
+
+void ReplaceMePIDSubsystem::InitDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // SetDefaultCommand(new MySpecialCommand());
+}
diff --git a/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.h b/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.h
new file mode 100644
index 0000000..2ab3d92
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/PIDSubsystem.h>
+
+class ReplaceMePIDSubsystem : public frc::PIDSubsystem {
+ public:
+ ReplaceMePIDSubsystem();
+ double ReturnPIDInput() override;
+ void UsePIDOutput(double output) override;
+ void InitDefaultCommand() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.cpp b/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.cpp
new file mode 100644
index 0000000..8ca89d3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "ReplaceMeSubsystem.h"
+
+ReplaceMeSubsystem::ReplaceMeSubsystem() : Subsystem("ExampleSubsystem") {}
+
+void ReplaceMeSubsystem::InitDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // SetDefaultCommand(new MySpecialCommand());
+}
+
+// Put methods for controlling this subsystem
+// here. Call these from Commands.
diff --git a/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.h b/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.h
new file mode 100644
index 0000000..de84a48
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Subsystem.h>
+
+class ReplaceMeSubsystem : public frc::Subsystem {
+ private:
+ // It's desirable that everything possible under private except
+ // for methods that implement subsystem capabilities
+
+ public:
+ ReplaceMeSubsystem();
+ void InitDefaultCommand() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.cpp b/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.cpp
new file mode 100644
index 0000000..2076e5b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "ReplaceMeTimedCommand.h"
+
+ReplaceMeTimedCommand::ReplaceMeTimedCommand(double timeout)
+ : TimedCommand(timeout) {
+ // Use Requires() here to declare subsystem dependencies
+ // eg. Requires(Robot::chassis.get());
+}
+
+// Called just before this Command runs the first time
+void ReplaceMeTimedCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void ReplaceMeTimedCommand::Execute() {}
+
+// Called once after command times out
+void ReplaceMeTimedCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void ReplaceMeTimedCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.h b/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.h
new file mode 100644
index 0000000..bf55e45
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/TimedCommand.h>
+
+class ReplaceMeTimedCommand : public frc::TimedCommand {
+ public:
+ explicit ReplaceMeTimedCommand(double timeout);
+ void Initialize() override;
+ void Execute() override;
+ void End() override;
+ void Interrupted() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.cpp b/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.cpp
new file mode 100644
index 0000000..4759e0e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.cpp
@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "ReplaceMeTrigger.h"
+
+ReplaceMeTrigger::ReplaceMeTrigger() {}
+
+bool ReplaceMeTrigger::Get() { return false; }
diff --git a/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.h b/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.h
new file mode 100644
index 0000000..9da7ef8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.h
@@ -0,0 +1,16 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/buttons/Trigger.h>
+
+class ReplaceMeTrigger : public frc::Trigger {
+ public:
+ ReplaceMeTrigger();
+ bool Get() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
new file mode 100644
index 0000000..3361754
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with arcade steering.
+ */
+class Robot : public frc::TimedRobot {
+ frc::PWMVictorSPX m_leftMotor{0};
+ frc::PWMVictorSPX m_rightMotor{1};
+ frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+ frc::Joystick m_stick{0};
+
+ public:
+ void TeleopPeriodic() {
+ // Drive with arcade style
+ m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/AxisCameraSample/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/AxisCameraSample/cpp/Robot.cpp
new file mode 100644
index 0000000..2811cf1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/AxisCameraSample/cpp/Robot.cpp
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <cameraserver/CameraServer.h>
+#include <frc/TimedRobot.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/types.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+/**
+ * This is a demo program showing the use of OpenCV to do vision processing. The
+ * image is acquired from the Axis camera, then a rectangle is put on the image
+ * and sent to the dashboard. OpenCV has many methods for different types of
+ * processing.
+ */
+class Robot : public frc::TimedRobot {
+ private:
+ static void VisionThread() {
+ // Get the Axis camera from CameraServer
+ cs::AxisCamera camera =
+ frc::CameraServer::GetInstance()->AddAxisCamera("axis-camera.local");
+ // Set the resolution
+ camera.SetResolution(640, 480);
+
+ // Get a CvSink. This will capture Mats from the Camera
+ cs::CvSink cvSink = frc::CameraServer::GetInstance()->GetVideo();
+ // Setup a CvSource. This will send images back to the Dashboard
+ cs::CvSource outputStream =
+ frc::CameraServer::GetInstance()->PutVideo("Rectangle", 640, 480);
+
+ // Mats are very memory expensive. Lets reuse this Mat.
+ cv::Mat mat;
+
+ while (true) {
+ // Tell the CvSink to grab a frame from the camera and put it in the
+ // source mat. If there is an error notify the output.
+ if (cvSink.GrabFrame(mat) == 0) {
+ // Send the output the error.
+ outputStream.NotifyError(cvSink.GetError());
+ // skip the rest of the current iteration
+ continue;
+ }
+ // Put a rectangle on the image
+ rectangle(mat, cv::Point(100, 100), cv::Point(400, 400),
+ cv::Scalar(255, 255, 255), 5);
+ // Give the output stream a new image to display
+ outputStream.PutFrame(mat);
+ }
+ }
+
+ void RobotInit() override {
+ // We need to run our vision program in a separate thread. If not, our robot
+ // program will not run.
+ std::thread visionThread(VisionThread);
+ visionThread.detach();
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/CANPDP/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/CANPDP/cpp/Robot.cpp
new file mode 100644
index 0000000..76493b5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/CANPDP/cpp/Robot.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/PowerDistributionPanel.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+/**
+ * This is a sample program showing how to retrieve information from the Power
+ * Distribution Panel via CAN. The information will be displayed under variables
+ * through the SmartDashboard.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void TeleopPeriodic() override {
+ /* Get the current going through channel 7, in Amperes. The PDP returns the
+ * current in increments of 0.125A. At low currents the current readings
+ * tend to be less accurate.
+ */
+ frc::SmartDashboard::PutNumber("Current Channel 7", m_pdp.GetCurrent(7));
+
+ /* Get the voltage going into the PDP, in Volts. The PDP returns the voltage
+ * in increments of 0.05 Volts.
+ */
+ frc::SmartDashboard::PutNumber("Voltage", m_pdp.GetVoltage());
+
+ // Retrieves the temperature of the PDP, in degrees Celsius.
+ frc::SmartDashboard::PutNumber("Temperature", m_pdp.GetTemperature());
+ }
+
+ private:
+ // Object for dealing with the Power Distribution Panel (PDP).
+ frc::PowerDistributionPanel m_pdp;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
new file mode 100644
index 0000000..1dbbfa5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Encoder.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+/**
+ * Sample program displaying the value of a quadrature encoder on the
+ * SmartDashboard.
+ *
+ * Quadrature Encoders are digital sensors which can detect the amount the
+ * encoder has rotated since starting as well as the direction in which the
+ * encoder shaft is rotating. However, encoders can not tell you the absolute
+ * position of the encoder shaft (ie, it considers where it starts to be the
+ * zero position, no matter where it starts), and so can only tell you how much
+ * the encoder has rotated since starting.
+ *
+ * Depending on the precision of an encoder, it will have fewer or greater ticks
+ * per revolution; the number of ticks per revolution will affect the conversion
+ * between ticks and distance, as specified by DistancePerPulse.
+ *
+ * One of the most common uses of encoders is in the drivetrain, so that the
+ * distance that the robot drives can be precisely controlled during the
+ * autonomous mode.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ Robot() {
+ /* Defines the number of samples to average when determining the rate.
+ * On a quadrature encoder, values range from 1-255; larger values result in
+ * smoother but potentially less accurate rates than lower values.
+ */
+ m_encoder.SetSamplesToAverage(5);
+
+ /* Defines how far the mechanism attached to the encoder moves per pulse. In
+ * this case, we assume that a 360 count encoder is directly attached to a 3
+ * inch diameter (1.5inch radius) wheel, and that we want to measure
+ * distance in inches.
+ */
+ m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5);
+
+ /* Defines the lowest rate at which the encoder will not be considered
+ * stopped, for the purposes of the GetStopped() method. Units are in
+ * distance / second, where distance refers to the units of distance that
+ * you are using, in this case inches.
+ */
+ m_encoder.SetMinRate(1.0);
+ }
+
+ void TeleopPeriodic() override {
+ // Retrieve the net displacement of the Encoder since the last Reset.
+ frc::SmartDashboard::PutNumber("Encoder Distance", m_encoder.GetDistance());
+
+ // Retrieve the current rate of the encoder.
+ frc::SmartDashboard::PutNumber("Encoder Rate", m_encoder.GetRate());
+ }
+
+ private:
+ /**
+ * The Encoder object is constructed with 4 parameters, the last two being
+ * optional.
+ *
+ * The first two parameters (1, 2 in this case) refer to the ports on the
+ * roboRIO which the encoder uses. Because a quadrature encoder has two signal
+ * wires, the signal from two DIO ports on the roboRIO are used.
+ *
+ * The third (optional) parameter is a boolean which defaults to false. If you
+ * set this parameter to true, the direction of the encoder will be reversed,
+ * in case it makes more sense mechanically.
+ *
+ * The final (optional) parameter specifies encoding rate (k1X, k2X, or k4X)
+ * and defaults to k4X. Faster (k4X) encoding gives greater positional
+ * precision but more noise in the rate.
+ */
+ frc::Encoder m_encoder{1, 2, false, frc::Encoder::k4X};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/OI.cpp
new file mode 100644
index 0000000..bd0e922
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/OI.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "OI.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+
+#include "commands/Autonomous.h"
+#include "commands/CloseClaw.h"
+#include "commands/OpenClaw.h"
+#include "commands/Pickup.h"
+#include "commands/Place.h"
+#include "commands/PrepareToPickup.h"
+#include "commands/SetElevatorSetpoint.h"
+
+OI::OI() {
+ frc::SmartDashboard::PutData("Open Claw", new OpenClaw());
+ frc::SmartDashboard::PutData("Close Claw", new CloseClaw());
+
+ // Connect the buttons to commands
+ m_dUp.WhenPressed(new SetElevatorSetpoint(0.2));
+ m_dDown.WhenPressed(new SetElevatorSetpoint(-0.2));
+ m_dRight.WhenPressed(new CloseClaw());
+ m_dLeft.WhenPressed(new OpenClaw());
+
+ m_r1.WhenPressed(new PrepareToPickup());
+ m_r2.WhenPressed(new Pickup());
+ m_l1.WhenPressed(new Place());
+ m_l2.WhenPressed(new Autonomous());
+}
+
+frc::Joystick& OI::GetJoystick() { return m_joy; }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
new file mode 100644
index 0000000..ca2f9d6
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <iostream>
+
+DriveTrain Robot::drivetrain;
+Elevator Robot::elevator;
+Wrist Robot::wrist;
+Claw Robot::claw;
+OI Robot::oi;
+
+void Robot::RobotInit() {
+ // Show what command your subsystem is running on the SmartDashboard
+ frc::SmartDashboard::PutData(&drivetrain);
+ frc::SmartDashboard::PutData(&elevator);
+ frc::SmartDashboard::PutData(&wrist);
+ frc::SmartDashboard::PutData(&claw);
+}
+
+void Robot::AutonomousInit() {
+ m_autonomousCommand.Start();
+ std::cout << "Starting Auto" << std::endl;
+}
+
+void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when teleop starts
+ // running. If you want the autonomous to continue until interrupted by
+ // another command, remove this line or comment it out.
+ m_autonomousCommand.Cancel();
+ std::cout << "Starting Teleop" << std::endl;
+}
+
+void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
new file mode 100644
index 0000000..30ce002
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/Autonomous.h"
+
+#include "commands/CloseClaw.h"
+#include "commands/DriveStraight.h"
+#include "commands/Pickup.h"
+#include "commands/Place.h"
+#include "commands/PrepareToPickup.h"
+#include "commands/SetDistanceToBox.h"
+#include "commands/SetWristSetpoint.h"
+
+Autonomous::Autonomous() : frc::CommandGroup("Autonomous") {
+ AddSequential(new PrepareToPickup());
+ AddSequential(new Pickup());
+ AddSequential(new SetDistanceToBox(0.10));
+ // AddSequential(new DriveStraight(4)); // Use Encoders if ultrasonic
+ // is broken
+ AddSequential(new Place());
+ AddSequential(new SetDistanceToBox(0.60));
+ // AddSequential(new DriveStraight(-2)); // Use Encoders if ultrasonic
+ // is broken
+ AddParallel(new SetWristSetpoint(-45));
+ AddSequential(new CloseClaw());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
new file mode 100644
index 0000000..e1a2f6e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/CloseClaw.h"
+
+#include "Robot.h"
+
+CloseClaw::CloseClaw() : frc::Command("CloseClaw") { Requires(&Robot::claw); }
+
+// Called just before this Command runs the first time
+void CloseClaw::Initialize() { Robot::claw.Close(); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool CloseClaw::IsFinished() { return Robot::claw.IsGripping(); }
+
+// Called once after isFinished returns true
+void CloseClaw::End() {
+// NOTE: Doesn't stop in simulation due to lower friction causing the can to
+// fall out
+// + there is no need to worry about stalling the motor or crushing the can.
+#ifndef SIMULATION
+ Robot::claw.Stop();
+#endif
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
new file mode 100644
index 0000000..dfd1744
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/DriveStraight.h"
+
+#include "Robot.h"
+
+DriveStraight::DriveStraight(double distance) {
+ Requires(&Robot::drivetrain);
+ m_pid.SetAbsoluteTolerance(0.01);
+ m_pid.SetSetpoint(distance);
+}
+
+// Called just before this Command runs the first time
+void DriveStraight::Initialize() {
+ // Get everything in a safe starting state.
+ Robot::drivetrain.Reset();
+ m_pid.Reset();
+ m_pid.Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool DriveStraight::IsFinished() { return m_pid.OnTarget(); }
+
+// Called once after isFinished returns true
+void DriveStraight::End() {
+ // Stop PID and the wheels
+ m_pid.Disable();
+ Robot::drivetrain.Drive(0, 0);
+}
+
+double DriveStraight::DriveStraightPIDSource::PIDGet() {
+ return Robot::drivetrain.GetDistance();
+}
+
+void DriveStraight::DriveStraightPIDOutput::PIDWrite(double d) {
+ Robot::drivetrain.Drive(d, d);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
new file mode 100644
index 0000000..175f3c1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/OpenClaw.h"
+
+#include "Robot.h"
+
+OpenClaw::OpenClaw() : frc::Command("OpenClaw") {
+ Requires(&Robot::claw);
+ SetTimeout(1);
+}
+
+// Called just before this Command runs the first time
+void OpenClaw::Initialize() { Robot::claw.Open(); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool OpenClaw::IsFinished() { return IsTimedOut(); }
+
+// Called once after isFinished returns true
+void OpenClaw::End() { Robot::claw.Stop(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
new file mode 100644
index 0000000..b5d8dd1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/Pickup.h"
+
+#include "commands/CloseClaw.h"
+#include "commands/SetElevatorSetpoint.h"
+#include "commands/SetWristSetpoint.h"
+
+Pickup::Pickup() : frc::CommandGroup("Pickup") {
+ AddSequential(new CloseClaw());
+ AddParallel(new SetWristSetpoint(-45));
+ AddSequential(new SetElevatorSetpoint(0.25));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
new file mode 100644
index 0000000..cf31d3c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/Place.h"
+
+#include "commands/OpenClaw.h"
+#include "commands/SetElevatorSetpoint.h"
+#include "commands/SetWristSetpoint.h"
+
+Place::Place() : frc::CommandGroup("Place") {
+ AddSequential(new SetElevatorSetpoint(0.25));
+ AddSequential(new SetWristSetpoint(0));
+ AddSequential(new OpenClaw());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
new file mode 100644
index 0000000..af8f0c7
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/PrepareToPickup.h"
+
+#include "commands/OpenClaw.h"
+#include "commands/SetElevatorSetpoint.h"
+#include "commands/SetWristSetpoint.h"
+
+PrepareToPickup::PrepareToPickup() : frc::CommandGroup("PrepareToPickup") {
+ AddParallel(new OpenClaw());
+ AddParallel(new SetWristSetpoint(0));
+ AddSequential(new SetElevatorSetpoint(0));
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
new file mode 100644
index 0000000..a0921e8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/SetDistanceToBox.h"
+
+#include <frc/PIDController.h>
+
+#include "Robot.h"
+
+SetDistanceToBox::SetDistanceToBox(double distance) {
+ Requires(&Robot::drivetrain);
+ m_pid.SetAbsoluteTolerance(0.01);
+ m_pid.SetSetpoint(distance);
+}
+
+// Called just before this Command runs the first time
+void SetDistanceToBox::Initialize() {
+ // Get everything in a safe starting state.
+ Robot::drivetrain.Reset();
+ m_pid.Reset();
+ m_pid.Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetDistanceToBox::IsFinished() { return m_pid.OnTarget(); }
+
+// Called once after isFinished returns true
+void SetDistanceToBox::End() {
+ // Stop PID and the wheels
+ m_pid.Disable();
+ Robot::drivetrain.Drive(0, 0);
+}
+
+double SetDistanceToBox::SetDistanceToBoxPIDSource::PIDGet() {
+ return Robot::drivetrain.GetDistanceToObstacle();
+}
+
+void SetDistanceToBox::SetDistanceToBoxPIDOutput::PIDWrite(double d) {
+ Robot::drivetrain.Drive(d, d);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
new file mode 100644
index 0000000..afe85e5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/SetElevatorSetpoint.h"
+
+#include <cmath>
+
+#include "Robot.h"
+
+SetElevatorSetpoint::SetElevatorSetpoint(double setpoint)
+ : frc::Command("SetElevatorSetpoint") {
+ m_setpoint = setpoint;
+ Requires(&Robot::elevator);
+}
+
+// Called just before this Command runs the first time
+void SetElevatorSetpoint::Initialize() {
+ Robot::elevator.SetSetpoint(m_setpoint);
+ Robot::elevator.Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetElevatorSetpoint::IsFinished() { return Robot::elevator.OnTarget(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
new file mode 100644
index 0000000..e9dc2af
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/SetWristSetpoint.h"
+
+#include "Robot.h"
+
+SetWristSetpoint::SetWristSetpoint(double setpoint)
+ : frc::Command("SetWristSetpoint") {
+ m_setpoint = setpoint;
+ Requires(&Robot::wrist);
+}
+
+// Called just before this Command runs the first time
+void SetWristSetpoint::Initialize() {
+ Robot::wrist.SetSetpoint(m_setpoint);
+ Robot::wrist.Enable();
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetWristSetpoint::IsFinished() { return Robot::wrist.OnTarget(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDriveWithJoystick.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDriveWithJoystick.cpp
new file mode 100644
index 0000000..a1be9a3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDriveWithJoystick.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/TankDriveWithJoystick.h"
+
+#include "Robot.h"
+
+TankDriveWithJoystick::TankDriveWithJoystick()
+ : frc::Command("TankDriveWithJoystick") {
+ Requires(&Robot::drivetrain);
+}
+
+// Called repeatedly when this Command is scheduled to run
+void TankDriveWithJoystick::Execute() {
+ auto& joystick = Robot::oi.GetJoystick();
+ Robot::drivetrain.Drive(-joystick.GetY(), -joystick.GetRawAxis(4));
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool TankDriveWithJoystick::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void TankDriveWithJoystick::End() { Robot::drivetrain.Drive(0, 0); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
new file mode 100644
index 0000000..8bad17f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/Claw.h"
+
+Claw::Claw() : frc::Subsystem("Claw") {
+ // Let's show everything on the LiveWindow
+ AddChild("Motor", m_motor);
+}
+
+void Claw::InitDefaultCommand() {}
+
+void Claw::Open() { m_motor.Set(-1); }
+
+void Claw::Close() { m_motor.Set(1); }
+
+void Claw::Stop() { m_motor.Set(0); }
+
+bool Claw::IsGripping() { return m_contact.Get(); }
+
+void Claw::Log() {}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
new file mode 100644
index 0000000..4110485
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/DriveTrain.h"
+
+#include <frc/Joystick.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+#include "commands/TankDriveWithJoystick.h"
+
+DriveTrain::DriveTrain() : frc::Subsystem("DriveTrain") {
+// Encoders may measure differently in the real world and in
+// simulation. In this example the robot moves 0.042 barleycorns
+// per tick in the real world, but the simulated encoders
+// simulate 360 tick encoders. This if statement allows for the
+// real robot to handle this difference in devices.
+#ifndef SIMULATION
+ m_leftEncoder.SetDistancePerPulse(0.042);
+ m_rightEncoder.SetDistancePerPulse(0.042);
+#else
+ // Circumference in ft = 4in/12(in/ft)*PI
+ m_leftEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
+ 360.0);
+ m_rightEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
+ 360.0);
+#endif
+
+ // Let's show everything on the LiveWindow
+ // AddChild("Front_Left Motor", m_frontLeft);
+ // AddChild("Rear Left Motor", m_rearLeft);
+ // AddChild("Front Right Motor", m_frontRight);
+ // AddChild("Rear Right Motor", m_rearRight);
+ AddChild("Left Encoder", m_leftEncoder);
+ AddChild("Right Encoder", m_rightEncoder);
+ AddChild("Rangefinder", m_rangefinder);
+ AddChild("Gyro", m_gyro);
+}
+
+void DriveTrain::InitDefaultCommand() {
+ SetDefaultCommand(new TankDriveWithJoystick());
+}
+
+void DriveTrain::Log() {
+ frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance());
+ frc::SmartDashboard::PutNumber("Right Distance",
+ m_rightEncoder.GetDistance());
+ frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate());
+ frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate());
+ frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle());
+}
+
+void DriveTrain::Drive(double left, double right) {
+ m_robotDrive.TankDrive(left, right);
+}
+
+double DriveTrain::GetHeading() { return m_gyro.GetAngle(); }
+
+void DriveTrain::Reset() {
+ m_gyro.Reset();
+ m_leftEncoder.Reset();
+ m_rightEncoder.Reset();
+}
+
+double DriveTrain::GetDistance() {
+ return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
+}
+
+double DriveTrain::GetDistanceToObstacle() {
+ // Really meters in simulation since it's a rangefinder...
+ return m_rangefinder.GetAverageVoltage();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
new file mode 100644
index 0000000..5318481
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/Elevator.h"
+
+#include <frc/livewindow/LiveWindow.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Elevator::Elevator() : frc::PIDSubsystem("Elevator", kP_real, kI_real, 0.0) {
+#ifdef SIMULATION // Check for simulation and update PID values
+ GetPIDController()->SetPID(kP_simulation, kI_simulation, 0, 0);
+#endif
+ SetAbsoluteTolerance(0.005);
+
+ // Let's show everything on the LiveWindow
+ AddChild("Motor", m_motor);
+ AddChild("Pot", &m_pot);
+}
+
+void Elevator::InitDefaultCommand() {}
+
+void Elevator::Log() {
+ // frc::SmartDashboard::PutData("Wrist Pot", &m_pot);
+}
+
+double Elevator::ReturnPIDInput() { return m_pot.Get(); }
+
+void Elevator::UsePIDOutput(double d) { m_motor.Set(d); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
new file mode 100644
index 0000000..9bab812
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/Wrist.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Wrist::Wrist() : frc::PIDSubsystem("Wrist", kP_real, 0.0, 0.0) {
+#ifdef SIMULATION // Check for simulation and update PID values
+ GetPIDController()->SetPID(kP_simulation, 0, 0, 0);
+#endif
+ SetAbsoluteTolerance(2.5);
+
+ // Let's show everything on the LiveWindow
+ AddChild("Motor", m_motor);
+ AddChild("Pot", m_pot);
+}
+
+void Wrist::InitDefaultCommand() {}
+
+void Wrist::Log() {
+ // frc::SmartDashboard::PutData("Wrist Angle", &m_pot);
+}
+
+double Wrist::ReturnPIDInput() { return m_pot.Get(); }
+
+void Wrist::UsePIDOutput(double d) { m_motor.Set(d); }
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/OI.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/OI.h
new file mode 100644
index 0000000..77e50b9
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/OI.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Joystick.h>
+#include <frc/buttons/JoystickButton.h>
+
+class OI {
+ public:
+ OI();
+ frc::Joystick& GetJoystick();
+
+ private:
+ frc::Joystick m_joy{0};
+
+ // Create some buttons
+ frc::JoystickButton m_dUp{&m_joy, 5};
+ frc::JoystickButton m_dRight{&m_joy, 6};
+ frc::JoystickButton m_dDown{&m_joy, 7};
+ frc::JoystickButton m_dLeft{&m_joy, 8};
+ frc::JoystickButton m_l2{&m_joy, 9};
+ frc::JoystickButton m_r2{&m_joy, 10};
+ frc::JoystickButton m_l1{&m_joy, 11};
+ frc::JoystickButton m_r1{&m_joy, 12};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
new file mode 100644
index 0000000..015bbc0
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/TimedRobot.h>
+#include <frc/commands/Command.h>
+#include <frc/commands/Scheduler.h>
+#include <frc/livewindow/LiveWindow.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+#include "OI.h"
+#include "commands/Autonomous.h"
+#include "subsystems/Claw.h"
+#include "subsystems/DriveTrain.h"
+#include "subsystems/Elevator.h"
+#include "subsystems/Wrist.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ static DriveTrain drivetrain;
+ static Elevator elevator;
+ static Wrist wrist;
+ static Claw claw;
+ static OI oi;
+
+ private:
+ Autonomous m_autonomousCommand;
+ frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
+
+ void RobotInit() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
new file mode 100644
index 0000000..e90098e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+/**
+ * The main autonomous command to pickup and deliver the soda to the box.
+ */
+class Autonomous : public frc::CommandGroup {
+ public:
+ Autonomous();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
new file mode 100644
index 0000000..bda8a5f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Opens the claw for one second. Real robots should use sensors, stalling
+ * motors is BAD!
+ */
+class CloseClaw : public frc::Command {
+ public:
+ CloseClaw();
+ void Initialize() override;
+ bool IsFinished() override;
+ void End() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
new file mode 100644
index 0000000..f03e46c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/PIDController.h>
+#include <frc/PIDOutput.h>
+#include <frc/PIDSource.h>
+#include <frc/commands/Command.h>
+
+/**
+ * Drive the given distance straight (negative values go backwards).
+ * Uses a local PID controller to run a simple PID loop that is only
+ * enabled while this command is running. The input is the averaged
+ * values of the left and right encoders.
+ */
+class DriveStraight : public frc::Command {
+ public:
+ explicit DriveStraight(double distance);
+ void Initialize() override;
+ bool IsFinished() override;
+ void End() override;
+
+ class DriveStraightPIDSource : public frc::PIDSource {
+ public:
+ virtual ~DriveStraightPIDSource() = default;
+ double PIDGet() override;
+ };
+
+ class DriveStraightPIDOutput : public frc::PIDOutput {
+ public:
+ virtual ~DriveStraightPIDOutput() = default;
+ void PIDWrite(double d) override;
+ };
+
+ private:
+ DriveStraightPIDSource m_source;
+ DriveStraightPIDOutput m_output;
+ frc::PIDController m_pid{4, 0, 0, &m_source, &m_output};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
new file mode 100644
index 0000000..c2a7cfb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Opens the claw for one second. Real robots should use sensors, stalling
+ * motors is BAD!
+ */
+class OpenClaw : public frc::Command {
+ public:
+ OpenClaw();
+ void Initialize() override;
+ bool IsFinished() override;
+ void End() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
new file mode 100644
index 0000000..ed68990
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+/**
+ * Pickup a soda can (if one is between the open claws) and
+ * get it in a safe state to drive around.
+ */
+class Pickup : public frc::CommandGroup {
+ public:
+ Pickup();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
new file mode 100644
index 0000000..7695393
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+/**
+ * Place a held soda can onto the platform.
+ */
+class Place : public frc::CommandGroup {
+ public:
+ Place();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
new file mode 100644
index 0000000..c58035d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+/**
+ * Make sure the robot is in a state to pickup soda cans.
+ */
+class PrepareToPickup : public frc::CommandGroup {
+ public:
+ PrepareToPickup();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
new file mode 100644
index 0000000..884a09b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/PIDController.h>
+#include <frc/PIDOutput.h>
+#include <frc/PIDSource.h>
+#include <frc/commands/Command.h>
+
+/**
+ * Drive until the robot is the given distance away from the box. Uses a local
+ * PID controller to run a simple PID loop that is only enabled while this
+ * command is running. The input is the averaged values of the left and right
+ * encoders.
+ */
+class SetDistanceToBox : public frc::Command {
+ public:
+ explicit SetDistanceToBox(double distance);
+ void Initialize() override;
+ bool IsFinished() override;
+ void End() override;
+
+ class SetDistanceToBoxPIDSource : public frc::PIDSource {
+ public:
+ virtual ~SetDistanceToBoxPIDSource() = default;
+ double PIDGet() override;
+ };
+
+ class SetDistanceToBoxPIDOutput : public frc::PIDOutput {
+ public:
+ virtual ~SetDistanceToBoxPIDOutput() = default;
+ void PIDWrite(double d) override;
+ };
+
+ private:
+ SetDistanceToBoxPIDSource m_source;
+ SetDistanceToBoxPIDOutput m_output;
+ frc::PIDController m_pid{-2, 0, 0, &m_source, &m_output};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
new file mode 100644
index 0000000..388ac0e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Move the elevator to a given location. This command finishes when it is
+ * within
+ * the tolerance, but leaves the PID loop running to maintain the position.
+ * Other
+ * commands using the elevator should make sure they disable PID!
+ */
+class SetElevatorSetpoint : public frc::Command {
+ public:
+ explicit SetElevatorSetpoint(double setpoint);
+ void Initialize() override;
+ bool IsFinished() override;
+
+ private:
+ double m_setpoint;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
new file mode 100644
index 0000000..effb173
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Move the wrist to a given angle. This command finishes when it is within
+ * the tolerance, but leaves the PID loop running to maintain the position.
+ * Other commands using the wrist should make sure they disable PID!
+ */
+class SetWristSetpoint : public frc::Command {
+ public:
+ explicit SetWristSetpoint(double setpoint);
+ void Initialize() override;
+ bool IsFinished() override;
+
+ private:
+ double m_setpoint;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDriveWithJoystick.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDriveWithJoystick.h
new file mode 100644
index 0000000..9337025
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDriveWithJoystick.h
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Have the robot drive tank style using the PS3 Joystick until interrupted.
+ */
+class TankDriveWithJoystick : public frc::Command {
+ public:
+ TankDriveWithJoystick();
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
new file mode 100644
index 0000000..96a436a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/DigitalInput.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/commands/Subsystem.h>
+
+/**
+ * The claw subsystem is a simple system with a motor for opening and closing.
+ * If using stronger motors, you should probably use a sensor so that the
+ * motors don't stall.
+ */
+class Claw : public frc::Subsystem {
+ public:
+ Claw();
+
+ void InitDefaultCommand() override;
+
+ /**
+ * Set the claw motor to move in the open direction.
+ */
+ void Open();
+
+ /**
+ * Set the claw motor to move in the close direction.
+ */
+ void Close();
+
+ /**
+ * Stops the claw motor from moving.
+ */
+ void Stop();
+
+ /**
+ * Return true when the robot is grabbing an object hard enough
+ * to trigger the limit switch.
+ */
+ bool IsGripping();
+
+ void Log();
+
+ private:
+ frc::PWMVictorSPX m_motor{7};
+ frc::DigitalInput m_contact{5};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
new file mode 100644
index 0000000..dce4d9c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogGyro.h>
+#include <frc/AnalogInput.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/commands/Subsystem.h>
+#include <frc/drive/DifferentialDrive.h>
+
+namespace frc {
+class Joystick;
+} // namespace frc
+
+/**
+ * The DriveTrain subsystem incorporates the sensors and actuators attached to
+ * the robots chassis. These include four drive motors, a left and right encoder
+ * and a gyro.
+ */
+class DriveTrain : public frc::Subsystem {
+ public:
+ DriveTrain();
+
+ /**
+ * When no other command is running let the operator drive around
+ * using the PS3 joystick.
+ */
+ void InitDefaultCommand() override;
+
+ /**
+ * The log method puts interesting information to the SmartDashboard.
+ */
+ void Log();
+
+ /**
+ * Tank style driving for the DriveTrain.
+ * @param left Speed in range [-1,1]
+ * @param right Speed in range [-1,1]
+ */
+ void Drive(double left, double right);
+
+ /**
+ * @return The robots heading in degrees.
+ */
+ double GetHeading();
+
+ /**
+ * Reset the robots sensors to the zero states.
+ */
+ void Reset();
+
+ /**
+ * @return The distance driven (average of left and right encoders).
+ */
+ double GetDistance();
+
+ /**
+ * @return The distance to the obstacle detected by the rangefinder.
+ */
+ double GetDistanceToObstacle();
+
+ private:
+ frc::PWMVictorSPX m_frontLeft{1};
+ frc::PWMVictorSPX m_rearLeft{2};
+ frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
+
+ frc::PWMVictorSPX m_frontRight{3};
+ frc::PWMVictorSPX m_rearRight{4};
+ frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
+
+ frc::DifferentialDrive m_robotDrive{m_left, m_right};
+
+ frc::Encoder m_leftEncoder{1, 2};
+ frc::Encoder m_rightEncoder{3, 4};
+ frc::AnalogInput m_rangefinder{6};
+ frc::AnalogGyro m_gyro{1};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
new file mode 100644
index 0000000..53f6057
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogPotentiometer.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/commands/PIDSubsystem.h>
+
+/**
+ * The elevator subsystem uses PID to go to a given height. Unfortunately, in
+ * it's current
+ * state PID values for simulation are different than in the real world do to
+ * minor differences.
+ */
+class Elevator : public frc::PIDSubsystem {
+ public:
+ Elevator();
+
+ void InitDefaultCommand() override;
+
+ /**
+ * The log method puts interesting information to the SmartDashboard.
+ */
+ void Log();
+
+ /**
+ * Use the potentiometer as the PID sensor. This method is automatically
+ * called by the subsystem.
+ */
+ double ReturnPIDInput() override;
+
+ /**
+ * Use the motor as the PID output. This method is automatically called
+ * by
+ * the subsystem.
+ */
+ void UsePIDOutput(double d) override;
+
+ private:
+ frc::PWMVictorSPX m_motor{5};
+
+// Conversion value of potentiometer varies between the real world and
+// simulation
+#ifndef SIMULATION
+ frc::AnalogPotentiometer m_pot{2, -2.0 / 5};
+#else
+ frc::AnalogPotentiometer m_pot{2}; // Defaults to meters
+#endif
+
+ static constexpr double kP_real = 4;
+ static constexpr double kI_real = 0.07;
+ static constexpr double kP_simulation = 18;
+ static constexpr double kI_simulation = 0.2;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
new file mode 100644
index 0000000..26fc74b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogPotentiometer.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/commands/PIDSubsystem.h>
+
+/**
+ * The wrist subsystem is like the elevator, but with a rotational joint instead
+ * of a linear joint.
+ */
+class Wrist : public frc::PIDSubsystem {
+ public:
+ Wrist();
+
+ void InitDefaultCommand() override;
+
+ /**
+ * The log method puts interesting information to the SmartDashboard.
+ */
+ void Log();
+
+ /**
+ * Use the potentiometer as the PID sensor. This method is automatically
+ * called by the subsystem.
+ */
+ double ReturnPIDInput() override;
+
+ /**
+ * Use the motor as the PID output. This method is automatically called
+ * by
+ * the subsystem.
+ */
+ void UsePIDOutput(double d) override;
+
+ private:
+ frc::PWMVictorSPX m_motor{6};
+
+// Conversion value of potentiometer varies between the real world and
+// simulation
+#ifndef SIMULATION
+ frc::AnalogPotentiometer m_pot{3, -270.0 / 5};
+#else
+ frc::AnalogPotentiometer m_pot{3}; // Defaults to degrees
+#endif
+
+ static constexpr double kP_real = 1;
+ static constexpr double kP_simulation = 0.05;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
new file mode 100644
index 0000000..f43cb57
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/livewindow/LiveWindow.h>
+
+class Robot : public frc::TimedRobot {
+ public:
+ Robot() {
+ m_robotDrive.SetExpiration(0.1);
+ m_timer.Start();
+ }
+
+ void AutonomousInit() override {
+ m_timer.Reset();
+ m_timer.Start();
+ }
+
+ void AutonomousPeriodic() override {
+ // Drive for 2 seconds
+ if (m_timer.Get() < 2.0) {
+ // Drive forwards half speed
+ m_robotDrive.ArcadeDrive(-0.5, 0.0);
+ } else {
+ // Stop robot
+ m_robotDrive.ArcadeDrive(0.0, 0.0);
+ }
+ }
+
+ void TeleopInit() override {}
+
+ void TeleopPeriodic() override {
+ // Drive with arcade style (use right stick)
+ m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
+ }
+
+ void TestPeriodic() override {}
+
+ private:
+ // Robot drive system
+ frc::PWMVictorSPX m_left{0};
+ frc::PWMVictorSPX m_right{1};
+ frc::DifferentialDrive m_robotDrive{m_left, m_right};
+
+ frc::Joystick m_stick{0};
+ frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
+ frc::Timer m_timer;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
new file mode 100644
index 0000000..b0c70d2
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <cmath>
+
+#include <frc/AnalogGyro.h>
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a sample program to demonstrate how to use a gyro sensor to make a
+ * robot drive straight. This program uses a joystick to drive forwards and
+ * backwards while the gyro is used for direction keeping.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override { m_gyro.SetSensitivity(kVoltsPerDegreePerSecond); }
+
+ /**
+ * The motor speed is set from the joystick while the DifferentialDrive
+ * turning value is assigned from the error between the setpoint and the gyro
+ * angle.
+ */
+ void TeleopPeriodic() override {
+ double turningValue = (kAngleSetpoint - m_gyro.GetAngle()) * kP;
+ // Invert the direction of the turn if we are going backwards
+ turningValue = std::copysign(turningValue, m_joystick.GetY());
+ m_robotDrive.ArcadeDrive(m_joystick.GetY(), turningValue);
+ }
+
+ private:
+ static constexpr double kAngleSetpoint = 0.0;
+ static constexpr double kP = 0.005; // Proportional turning constant
+
+ // Gyro calibration constant, may need to be adjusted. Gyro value of 360 is
+ // set to correspond to one full revolution.
+ static constexpr double kVoltsPerDegreePerSecond = 0.0128;
+
+ static constexpr int kLeftMotorPort = 0;
+ static constexpr int kRightMotorPort = 1;
+ static constexpr int kGyroPort = 0;
+ static constexpr int kJoystickPort = 0;
+
+ frc::PWMVictorSPX m_left{kLeftMotorPort};
+ frc::PWMVictorSPX m_right{kRightMotorPort};
+ frc::DifferentialDrive m_robotDrive{m_left, m_right};
+
+ frc::AnalogGyro m_gyro{kGyroPort};
+ frc::Joystick m_joystick{kJoystickPort};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
new file mode 100644
index 0000000..738ff32
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogGyro.h>
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/MecanumDrive.h>
+
+/**
+ * This is a sample program that uses mecanum drive with a gyro sensor to
+ * maintian rotation vectorsin relation to the starting orientation of the robot
+ * (field-oriented controls).
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override {
+ // Invert the left side motors. You may need to change or remove this to
+ // match your robot.
+ m_frontLeft.SetInverted(true);
+ m_rearLeft.SetInverted(true);
+
+ m_gyro.SetSensitivity(kVoltsPerDegreePerSecond);
+ }
+
+ /**
+ * Mecanum drive is used with the gyro angle as an input.
+ */
+ void TeleopPeriodic() override {
+ m_robotDrive.DriveCartesian(m_joystick.GetX(), m_joystick.GetY(),
+ m_joystick.GetZ(), m_gyro.GetAngle());
+ }
+
+ private:
+ // Gyro calibration constant, may need to be adjusted. Gyro value of 360 is
+ // set to correspond to one full revolution.
+ static constexpr double kVoltsPerDegreePerSecond = 0.0128;
+
+ static constexpr int kFrontLeftMotorPort = 0;
+ static constexpr int kRearLeftMotorPort = 1;
+ static constexpr int kFrontRightMotorPort = 2;
+ static constexpr int kRearRightMotorPort = 3;
+ static constexpr int kGyroPort = 0;
+ static constexpr int kJoystickPort = 0;
+
+ frc::PWMVictorSPX m_frontLeft{kFrontLeftMotorPort};
+ frc::PWMVictorSPX m_rearLeft{kRearLeftMotorPort};
+ frc::PWMVictorSPX m_frontRight{kFrontRightMotorPort};
+ frc::PWMVictorSPX m_rearRight{kRearRightMotorPort};
+ frc::MecanumDrive m_robotDrive{m_frontLeft, m_rearLeft, m_frontRight,
+ m_rearRight};
+
+ frc::AnalogGyro m_gyro{kGyroPort};
+ frc::Joystick m_joystick{kJoystickPort};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
new file mode 100644
index 0000000..dedb0f2
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+/*
+This example shows how to use the HAL directly, and what is needed to run a
+basic program. The sample is compiled in C, however the functionality works from
+C++ as well.
+
+The HAL is considered a stable but changeable API. The API is stable during a
+season, and is safe to use for events. However, between seasons there might be
+changes to the API. This is an advanced sample, and should only be used by users
+that want even more control over what code runs on their robot.
+*/
+
+#include <stdio.h>
+
+#include "hal/HAL.h"
+
+enum DriverStationMode {
+ DisabledMode,
+ TeleopMode,
+ TestMode,
+ AutoMode,
+};
+
+enum DriverStationMode getDSMode(void) {
+ // Get Robot State
+ HAL_ControlWord word;
+ HAL_GetControlWord(&word);
+
+ // We send the observes, otherwise the DS disables
+ if (!word.enabled) {
+ HAL_ObserveUserProgramDisabled();
+ return DisabledMode;
+ } else {
+ if (word.autonomous) {
+ HAL_ObserveUserProgramAutonomous();
+ return AutoMode;
+ } else if (word.test) {
+ HAL_ObserveUserProgramTest();
+ return TestMode;
+ } else {
+ HAL_ObserveUserProgramTeleop();
+ return TeleopMode;
+ }
+ }
+}
+
+int main(void) {
+ // Must initialize the HAL, 500ms timeout
+ HAL_Bool initialized = HAL_Initialize(500, 0);
+ if (!initialized) {
+ printf("Failed to initialize the HAL\n");
+ return 1;
+ }
+
+ int32_t status = 0;
+
+ // For DS to see valid robot code
+ HAL_ObserveUserProgramStarting();
+
+ // Create a Motor Controller
+ status = 0;
+ HAL_DigitalHandle pwmPort = HAL_InitializePWMPort(HAL_GetPort(2), &status);
+
+ if (status != 0) {
+ const char* message = HAL_GetErrorMessage(status);
+ printf("%s\n", message);
+ return 1;
+ }
+
+ // Set PWM config to standard servo speeds
+ HAL_SetPWMConfig(pwmPort, 2.0, 1.501, 1.5, 1.499, 1.0, &status);
+
+ // Create an Input
+ status = 0;
+ HAL_DigitalHandle dio = HAL_InitializeDIOPort(HAL_GetPort(2), 1, &status);
+
+ if (status != 0) {
+ const char* message = HAL_GetErrorMessage(status);
+ printf("%s\n", message);
+ status = 0;
+ HAL_FreePWMPort(pwmPort, &status);
+ return 1;
+ }
+
+ while (1) {
+ // Wait for DS data, with a timeout
+ HAL_Bool validData = HAL_WaitForDSDataTimeout(1.0);
+ if (!validData) {
+ // Do something here on no packet
+ continue;
+ }
+ enum DriverStationMode dsMode = getDSMode();
+ switch (dsMode) {
+ case DisabledMode:
+ break;
+ case TeleopMode:
+ status = 0;
+ if (HAL_GetDIO(dio, &status)) {
+ HAL_SetPWMSpeed(pwmPort, 1.0, &status);
+ } else {
+ HAL_SetPWMSpeed(pwmPort, 0, &status);
+ }
+ break;
+ case AutoMode:
+ break;
+ case TestMode:
+ break;
+ default:
+ break;
+ }
+ }
+
+ // Clean up resources
+ status = 0;
+ HAL_FreeDIOPort(dio);
+ HAL_FreePWMPort(pwmPort, &status);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/HidRumble/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/HidRumble/cpp/Robot.cpp
new file mode 100644
index 0000000..3deafa5
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/HidRumble/cpp/Robot.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/GenericHID.h>
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+
+/**
+ * This is a demo program showing the use of GenericHID's rumble feature.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void AutonomousInit() override {
+ // Turn on rumble at the start of auto
+ m_hid.SetRumble(frc::GenericHID::RumbleType::kLeftRumble, 1.0);
+ m_hid.SetRumble(frc::GenericHID::RumbleType::kRightRumble, 1.0);
+ }
+
+ void DisabledInit() override {
+ // Stop the rumble when entering disabled
+ m_hid.SetRumble(frc::GenericHID::RumbleType::kLeftRumble, 0.0);
+ m_hid.SetRumble(frc::GenericHID::RumbleType::kRightRumble, 0.0);
+ }
+
+ private:
+ frc::XboxController m_hid{0};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp
new file mode 100644
index 0000000..ae2ac83
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <thread>
+
+#include <cameraserver/CameraServer.h>
+#include <frc/TimedRobot.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/types.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <wpi/raw_ostream.h>
+
+/**
+ * This is a demo program showing the use of OpenCV to do vision processing. The
+ * image is acquired from the USB camera, then a rectangle is put on the image
+ * and sent to the dashboard. OpenCV has many methods for different types of
+ * processing.
+ */
+class Robot : public frc::TimedRobot {
+#if defined(__linux__)
+
+ private:
+ static void VisionThread() {
+ // Get the USB camera from CameraServer
+ cs::UsbCamera camera =
+ frc::CameraServer::GetInstance()->StartAutomaticCapture();
+ // Set the resolution
+ camera.SetResolution(640, 480);
+
+ // Get a CvSink. This will capture Mats from the Camera
+ cs::CvSink cvSink = frc::CameraServer::GetInstance()->GetVideo();
+ // Setup a CvSource. This will send images back to the Dashboard
+ cs::CvSource outputStream =
+ frc::CameraServer::GetInstance()->PutVideo("Rectangle", 640, 480);
+
+ // Mats are very memory expensive. Lets reuse this Mat.
+ cv::Mat mat;
+
+ while (true) {
+ // Tell the CvSink to grab a frame from the camera and
+ // put it
+ // in the source mat. If there is an error notify the
+ // output.
+ if (cvSink.GrabFrame(mat) == 0) {
+ // Send the output the error.
+ outputStream.NotifyError(cvSink.GetError());
+ // skip the rest of the current iteration
+ continue;
+ }
+ // Put a rectangle on the image
+ rectangle(mat, cv::Point(100, 100), cv::Point(400, 400),
+ cv::Scalar(255, 255, 255), 5);
+ // Give the output stream a new image to display
+ outputStream.PutFrame(mat);
+ }
+ }
+#endif
+
+ void RobotInit() override {
+ // We need to run our vision program in a separate thread. If not, our robot
+ // program will not run.
+#if defined(__linux__)
+ std::thread visionThread(VisionThread);
+ visionThread.detach();
+#else
+ wpi::errs() << "Vision only available on Linux.\n";
+ wpi::errs().flush();
+#endif
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
new file mode 100644
index 0000000..8b4c98c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/MecanumDrive.h>
+
+/**
+ * This is a demo program showing how to use Mecanum control with the
+ * MecanumDrive class.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override {
+ // Invert the left side motors. You may need to change or remove this to
+ // match your robot.
+ m_frontLeft.SetInverted(true);
+ m_rearLeft.SetInverted(true);
+ }
+
+ void TeleopPeriodic() override {
+ /* Use the joystick X axis for lateral movement, Y axis for forward
+ * movement, and Z axis for rotation.
+ */
+ m_robotDrive.DriveCartesian(m_stick.GetX(), m_stick.GetY(), m_stick.GetZ());
+ }
+
+ private:
+ static constexpr int kFrontLeftChannel = 0;
+ static constexpr int kRearLeftChannel = 1;
+ static constexpr int kFrontRightChannel = 2;
+ static constexpr int kRearRightChannel = 3;
+
+ static constexpr int kJoystickChannel = 0;
+
+ frc::PWMVictorSPX m_frontLeft{kFrontLeftChannel};
+ frc::PWMVictorSPX m_rearLeft{kRearLeftChannel};
+ frc::PWMVictorSPX m_frontRight{kFrontRightChannel};
+ frc::PWMVictorSPX m_rearRight{kRearRightChannel};
+ frc::MecanumDrive m_robotDrive{m_frontLeft, m_rearLeft, m_frontRight,
+ m_rearRight};
+
+ frc::Joystick m_stick{kJoystickChannel};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/MotorControl/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MotorControl/cpp/Robot.cpp
new file mode 100644
index 0000000..2217ec1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MotorControl/cpp/Robot.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * This sample program shows how to control a motor using a joystick. In the
+ * operator control part of the program, the joystick is read and the value is
+ * written to the motor.
+ *
+ * Joystick analog values range from -1 to 1 and speed controller inputs as
+ * range from -1 to 1 making it easy to work together.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void TeleopPeriodic() override { m_motor.Set(m_stick.GetY()); }
+
+ private:
+ frc::Joystick m_stick{0};
+ frc::PWMVictorSPX m_motor{0};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp
new file mode 100644
index 0000000..78e1e96
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+constexpr double kPi = 3.14159265358979;
+
+/**
+ * This sample program shows how to control a motor using a joystick. In the
+ * operator control part of the program, the joystick is read and the value is
+ * written to the motor.
+ *
+ * Joystick analog values range from -1 to 1 and speed controller inputs as
+ * range from -1 to 1 making it easy to work together.
+ *
+ * In addition, the encoder value of an encoder connected to ports 0 and 1 is
+ * consistently sent to the Dashboard.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void TeleopPeriodic() override { m_motor.Set(m_stick.GetY()); }
+
+ /*
+ * The RobotPeriodic function is called every control packet no matter the
+ * robot mode.
+ */
+ void RobotPeriodic() override {
+ frc::SmartDashboard::PutNumber("Encoder", m_encoder.GetDistance());
+ }
+
+ void RobotInit() override {
+ // Use SetDistancePerPulse to set the multiplier for GetDistance
+ // This is set up assuming a 6 inch wheel with a 360 CPR encoder.
+ m_encoder.SetDistancePerPulse((kPi * 6) / 360.0);
+ }
+
+ private:
+ frc::Joystick m_stick{0};
+ frc::PWMVictorSPX m_motor{0};
+ frc::Encoder m_encoder{0, 1};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/OI.cpp
new file mode 100644
index 0000000..c5fcb15
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/OI.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "OI.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+
+#include "commands/Collect.h"
+#include "commands/DriveForward.h"
+#include "commands/LowGoal.h"
+#include "commands/SetCollectionSpeed.h"
+#include "commands/SetPivotSetpoint.h"
+#include "commands/Shoot.h"
+#include "subsystems/Collector.h"
+#include "subsystems/Pivot.h"
+
+OI::OI() {
+ m_r1.WhenPressed(new LowGoal());
+ m_r2.WhenPressed(new Collect());
+
+ m_l1.WhenPressed(new SetPivotSetpoint(Pivot::kShoot));
+ m_l2.WhenPressed(new SetPivotSetpoint(Pivot::kShootNear));
+
+ m_sticks.WhenActive(new Shoot());
+
+ // SmartDashboard Buttons
+ frc::SmartDashboard::PutData("Drive Forward", new DriveForward(2.25));
+ frc::SmartDashboard::PutData("Drive Backward", new DriveForward(-2.25));
+ frc::SmartDashboard::PutData("Start Rollers",
+ new SetCollectionSpeed(Collector::kForward));
+ frc::SmartDashboard::PutData("Stop Rollers",
+ new SetCollectionSpeed(Collector::kStop));
+ frc::SmartDashboard::PutData("Reverse Rollers",
+ new SetCollectionSpeed(Collector::kReverse));
+}
+
+frc::Joystick& OI::GetJoystick() { return m_joystick; }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp
new file mode 100644
index 0000000..9da27f3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <iostream>
+
+#include <frc/commands/Scheduler.h>
+#include <frc/livewindow/LiveWindow.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+DriveTrain Robot::drivetrain;
+Pivot Robot::pivot;
+Collector Robot::collector;
+Shooter Robot::shooter;
+Pneumatics Robot::pneumatics;
+OI Robot::oi;
+
+void Robot::RobotInit() {
+ // Show what command your subsystem is running on the SmartDashboard
+ frc::SmartDashboard::PutData(&drivetrain);
+ frc::SmartDashboard::PutData(&pivot);
+ frc::SmartDashboard::PutData(&collector);
+ frc::SmartDashboard::PutData(&shooter);
+ frc::SmartDashboard::PutData(&pneumatics);
+
+ // instantiate the command used for the autonomous period
+ m_autoChooser.SetDefaultOption("Drive and Shoot", &m_driveAndShootAuto);
+ m_autoChooser.AddOption("Drive Forward", &m_driveForwardAuto);
+ frc::SmartDashboard::PutData("Auto Mode", &m_autoChooser);
+
+ pneumatics.Start(); // Pressurize the pneumatics.
+}
+
+void Robot::AutonomousInit() {
+ m_autonomousCommand = m_autoChooser.GetSelected();
+ m_autonomousCommand->Start();
+}
+
+void Robot::AutonomousPeriodic() {
+ frc::Scheduler::GetInstance()->Run();
+ Log();
+}
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ }
+ std::cout << "Starting Teleop" << std::endl;
+}
+
+void Robot::TeleopPeriodic() {
+ frc::Scheduler::GetInstance()->Run();
+ Log();
+}
+
+void Robot::TestPeriodic() {}
+
+void Robot::DisabledInit() { shooter.Unlatch(); }
+
+void Robot::DisabledPeriodic() { Log(); }
+
+/**
+ * Log interesting values to the SmartDashboard.
+ */
+void Robot::Log() {
+ Robot::pneumatics.WritePressure();
+ frc::SmartDashboard::PutNumber("Pivot Pot Value", pivot.GetAngle());
+ frc::SmartDashboard::PutNumber("Left Distance",
+ drivetrain.GetLeftEncoder().GetDistance());
+ frc::SmartDashboard::PutNumber("Right Distance",
+ drivetrain.GetRightEncoder().GetDistance());
+}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CheckForHotGoal.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CheckForHotGoal.cpp
new file mode 100644
index 0000000..c5d80c0
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CheckForHotGoal.cpp
@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/CheckForHotGoal.h"
+
+#include "Robot.h"
+
+CheckForHotGoal::CheckForHotGoal(double time) { SetTimeout(time); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool CheckForHotGoal::IsFinished() {
+ return IsTimedOut() || Robot::shooter.GoalIsHot();
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CloseClaw.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CloseClaw.cpp
new file mode 100644
index 0000000..8db41f3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CloseClaw.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/CloseClaw.h"
+
+#include "Robot.h"
+
+CloseClaw::CloseClaw() { Requires(&Robot::collector); }
+
+// Called just before this Command runs the first time
+void CloseClaw::Initialize() { Robot::collector.Close(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Collect.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Collect.cpp
new file mode 100644
index 0000000..b51c2df
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Collect.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/Collect.h"
+
+#include "Robot.h"
+#include "commands/CloseClaw.h"
+#include "commands/SetCollectionSpeed.h"
+#include "commands/SetPivotSetpoint.h"
+#include "commands/WaitForBall.h"
+
+Collect::Collect() {
+ AddSequential(new SetCollectionSpeed(Collector::kForward));
+ AddParallel(new CloseClaw());
+ AddSequential(new SetPivotSetpoint(Pivot::kCollect));
+ AddSequential(new WaitForBall());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveAndShootAutonomous.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveAndShootAutonomous.cpp
new file mode 100644
index 0000000..3486292
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveAndShootAutonomous.cpp
@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/DriveAndShootAutonomous.h"
+
+#include "Robot.h"
+#include "commands/CheckForHotGoal.h"
+#include "commands/CloseClaw.h"
+#include "commands/DriveForward.h"
+#include "commands/SetPivotSetpoint.h"
+#include "commands/Shoot.h"
+#include "commands/WaitForPressure.h"
+
+DriveAndShootAutonomous::DriveAndShootAutonomous() {
+ AddSequential(new CloseClaw());
+ AddSequential(new WaitForPressure(), 2);
+#ifndef SIMULATION
+ // NOTE: Simulation doesn't currently have the concept of hot.
+ AddSequential(new CheckForHotGoal(2));
+#endif
+ AddSequential(new SetPivotSetpoint(45));
+ AddSequential(new DriveForward(8, 0.3));
+ AddSequential(new Shoot());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveForward.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveForward.cpp
new file mode 100644
index 0000000..1de5846
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveForward.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/DriveForward.h"
+
+#include <cmath>
+
+#include "Robot.h"
+
+void DriveForward::init(double dist, double maxSpeed) {
+ Requires(&Robot::drivetrain);
+ m_distance = dist;
+ m_driveForwardSpeed = maxSpeed;
+}
+
+DriveForward::DriveForward() { init(10, 0.5); }
+
+DriveForward::DriveForward(double dist) { init(dist, 0.5); }
+
+DriveForward::DriveForward(double dist, double maxSpeed) {
+ init(dist, maxSpeed);
+}
+
+// Called just before this Command runs the first time
+void DriveForward::Initialize() {
+ Robot::drivetrain.GetRightEncoder().Reset();
+ SetTimeout(2);
+}
+
+// Called repeatedly when this Command is scheduled to run
+void DriveForward::Execute() {
+ m_error = (m_distance - Robot::drivetrain.GetRightEncoder().GetDistance());
+ if (m_driveForwardSpeed * kP * m_error >= m_driveForwardSpeed) {
+ Robot::drivetrain.TankDrive(m_driveForwardSpeed, m_driveForwardSpeed);
+ } else {
+ Robot::drivetrain.TankDrive(m_driveForwardSpeed * kP * m_error,
+ m_driveForwardSpeed * kP * m_error);
+ }
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool DriveForward::IsFinished() {
+ return (std::fabs(m_error) <= kTolerance) || IsTimedOut();
+}
+
+// Called once after isFinished returns true
+void DriveForward::End() { Robot::drivetrain.Stop(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveWithJoystick.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveWithJoystick.cpp
new file mode 100644
index 0000000..7704d70
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveWithJoystick.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/DriveWithJoystick.h"
+
+#include "Robot.h"
+
+DriveWithJoystick::DriveWithJoystick() { Requires(&Robot::drivetrain); }
+
+// Called repeatedly when this Command is scheduled to run
+void DriveWithJoystick::Execute() {
+ auto& joystick = Robot::oi.GetJoystick();
+ Robot::drivetrain.TankDrive(joystick.GetY(), joystick.GetRawAxis(4));
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool DriveWithJoystick::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void DriveWithJoystick::End() { Robot::drivetrain.Stop(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/ExtendShooter.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/ExtendShooter.cpp
new file mode 100644
index 0000000..53494c9
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/ExtendShooter.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/ExtendShooter.h"
+
+#include "Robot.h"
+
+ExtendShooter::ExtendShooter() : frc::TimedCommand(1.0) {
+ Requires(&Robot::shooter);
+}
+
+// Called just before this Command runs the first time
+void ExtendShooter::Initialize() { Robot::shooter.ExtendBoth(); }
+
+// Called once after isFinished returns true
+void ExtendShooter::End() { Robot::shooter.RetractBoth(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/LowGoal.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/LowGoal.cpp
new file mode 100644
index 0000000..f4ed788
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/LowGoal.cpp
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/LowGoal.h"
+
+#include "Robot.h"
+#include "commands/ExtendShooter.h"
+#include "commands/SetCollectionSpeed.h"
+#include "commands/SetPivotSetpoint.h"
+
+LowGoal::LowGoal() {
+ AddSequential(new SetPivotSetpoint(Pivot::kLowGoal));
+ AddSequential(new SetCollectionSpeed(Collector::kReverse));
+ AddSequential(new ExtendShooter());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/OpenClaw.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/OpenClaw.cpp
new file mode 100644
index 0000000..4e4a9c8
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/OpenClaw.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/OpenClaw.h"
+
+#include "Robot.h"
+
+OpenClaw::OpenClaw() { Requires(&Robot::collector); }
+
+// Called just before this Command runs the first time
+void OpenClaw::Initialize() { Robot::collector.Open(); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool OpenClaw::IsFinished() { return Robot::collector.IsOpen(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetCollectionSpeed.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetCollectionSpeed.cpp
new file mode 100644
index 0000000..2011a02
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetCollectionSpeed.cpp
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/SetCollectionSpeed.h"
+
+#include "Robot.h"
+
+SetCollectionSpeed::SetCollectionSpeed(double speed) {
+ Requires(&Robot::collector);
+ m_speed = speed;
+}
+
+// Called just before this Command runs the first time
+void SetCollectionSpeed::Initialize() { Robot::collector.SetSpeed(m_speed); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetPivotSetpoint.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetPivotSetpoint.cpp
new file mode 100644
index 0000000..f9929de
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetPivotSetpoint.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/SetPivotSetpoint.h"
+
+#include "Robot.h"
+
+SetPivotSetpoint::SetPivotSetpoint(double setpoint) {
+ m_setpoint = setpoint;
+ Requires(&Robot::pivot);
+}
+
+// Called just before this Command runs the first time
+void SetPivotSetpoint::Initialize() {
+ Robot::pivot.Enable();
+ Robot::pivot.SetSetpoint(m_setpoint);
+}
+
+// Make this return true when this Command no longer needs to run execute()
+bool SetPivotSetpoint::IsFinished() { return Robot::pivot.OnTarget(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Shoot.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Shoot.cpp
new file mode 100644
index 0000000..836ccd2
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Shoot.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/Shoot.h"
+
+#include "Robot.h"
+#include "commands/ExtendShooter.h"
+#include "commands/OpenClaw.h"
+#include "commands/SetCollectionSpeed.h"
+#include "commands/WaitForPressure.h"
+
+Shoot::Shoot() {
+ AddSequential(new WaitForPressure());
+ AddSequential(new SetCollectionSpeed(Collector::kStop));
+ AddSequential(new OpenClaw());
+ AddSequential(new ExtendShooter());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForBall.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForBall.cpp
new file mode 100644
index 0000000..daf9665
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForBall.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/WaitForBall.h"
+
+#include "Robot.h"
+
+WaitForBall::WaitForBall() { Requires(&Robot::collector); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool WaitForBall::IsFinished() { return Robot::collector.HasBall(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForPressure.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForPressure.cpp
new file mode 100644
index 0000000..00cc454
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForPressure.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/WaitForPressure.h"
+
+#include "Robot.h"
+
+WaitForPressure::WaitForPressure() { Requires(&Robot::pneumatics); }
+
+// Make this return true when this Command no longer needs to run execute()
+bool WaitForPressure::IsFinished() { return Robot::pneumatics.IsPressurized(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Collector.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Collector.cpp
new file mode 100644
index 0000000..8f345ce
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Collector.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/Collector.h"
+
+#include <frc/livewindow/LiveWindow.h>
+
+Collector::Collector() : frc::Subsystem("Collector") {
+ // Put everything to the LiveWindow for testing.
+ AddChild("Roller Motor", m_rollerMotor);
+ AddChild("Ball Detector", m_ballDetector);
+ AddChild("Claw Open Detector", m_openDetector);
+ AddChild("Piston", m_piston);
+}
+
+bool Collector::HasBall() {
+ return m_ballDetector.Get(); // TODO: prepend ! to reflect real robot
+}
+
+void Collector::SetSpeed(double speed) { m_rollerMotor.Set(-speed); }
+
+void Collector::Stop() { m_rollerMotor.Set(0); }
+
+bool Collector::IsOpen() {
+ return m_openDetector.Get(); // TODO: prepend ! to reflect real robot
+}
+
+void Collector::Open() { m_piston.Set(true); }
+
+void Collector::Close() { m_piston.Set(false); }
+
+void Collector::InitDefaultCommand() {}
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp
new file mode 100644
index 0000000..53cb625
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/DriveTrain.h"
+
+#include <cmath>
+
+#include <frc/Joystick.h>
+
+#include "commands/DriveWithJoystick.h"
+
+DriveTrain::DriveTrain() : frc::Subsystem("DriveTrain") {
+ // AddChild("Front Left CIM", m_frontLeftCIM);
+ // AddChild("Front Right CIM", m_frontRightCIM);
+ // AddChild("Back Left CIM", m_backLeftCIM);
+ // AddChild("Back Right CIM", m_backRightCIM);
+
+ // Configure the DifferentialDrive to reflect the fact that all our
+ // motors are wired backwards and our drivers sensitivity preferences.
+ m_robotDrive.SetSafetyEnabled(false);
+ m_robotDrive.SetExpiration(0.1);
+ m_robotDrive.SetMaxOutput(1.0);
+ m_leftCIMs.SetInverted(true);
+ m_rightCIMs.SetInverted(true);
+
+ // Configure encoders
+ m_rightEncoder.SetPIDSourceType(frc::PIDSourceType::kDisplacement);
+ m_leftEncoder.SetPIDSourceType(frc::PIDSourceType::kDisplacement);
+
+#ifndef SIMULATION
+ // Converts to feet
+ m_rightEncoder.SetDistancePerPulse(0.0785398);
+ m_leftEncoder.SetDistancePerPulse(0.0785398);
+#else
+ // Convert to feet 4in diameter wheels with 360 tick simulated encoders
+ m_rightEncoder.SetDistancePerPulse((4.0 /*in*/ * M_PI) /
+ (360.0 * 12.0 /*in/ft*/));
+ m_leftEncoder.SetDistancePerPulse((4.0 /*in*/ * M_PI) /
+ (360.0 * 12.0 /*in/ft*/));
+#endif
+
+ AddChild("Right Encoder", m_rightEncoder);
+ AddChild("Left Encoder", m_leftEncoder);
+
+// Configure gyro
+#ifndef SIMULATION
+ m_gyro.SetSensitivity(0.007); // TODO: Handle more gracefully?
+#endif
+ AddChild("Gyro", m_gyro);
+}
+
+void DriveTrain::InitDefaultCommand() {
+ SetDefaultCommand(new DriveWithJoystick());
+}
+
+void DriveTrain::TankDrive(double leftAxis, double rightAxis) {
+ m_robotDrive.TankDrive(leftAxis, rightAxis);
+}
+
+void DriveTrain::Stop() { m_robotDrive.TankDrive(0.0, 0.0); }
+
+frc::Encoder& DriveTrain::GetLeftEncoder() { return m_leftEncoder; }
+
+frc::Encoder& DriveTrain::GetRightEncoder() { return m_rightEncoder; }
+
+double DriveTrain::GetAngle() { return m_gyro.GetAngle(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pivot.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pivot.cpp
new file mode 100644
index 0000000..861d583
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pivot.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/Pivot.h"
+
+Pivot::Pivot() : frc::PIDSubsystem("Pivot", 7.0, 0.0, 8.0) {
+ SetAbsoluteTolerance(0.005);
+ GetPIDController()->SetContinuous(false);
+#ifdef SIMULATION
+ // PID is different in simulation.
+ GetPIDController()->SetPID(0.5, 0.001, 2);
+ SetAbsoluteTolerance(5);
+#endif
+
+ // Put everything to the LiveWindow for testing.
+ AddChild("Upper Limit Switch", m_upperLimitSwitch);
+ AddChild("Lower Limit Switch", m_lowerLimitSwitch);
+ AddChild("Pot", m_pot);
+ AddChild("Motor", m_motor);
+}
+
+void InitDefaultCommand() {}
+
+double Pivot::ReturnPIDInput() { return m_pot.Get(); }
+
+void Pivot::UsePIDOutput(double output) { m_motor.PIDWrite(output); }
+
+bool Pivot::IsAtUpperLimit() {
+ return m_upperLimitSwitch.Get(); // TODO: inverted from real robot
+ // (prefix with !)
+}
+
+bool Pivot::IsAtLowerLimit() {
+ return m_lowerLimitSwitch.Get(); // TODO: inverted from real robot
+ // (prefix with !)
+}
+
+double Pivot::GetAngle() { return m_pot.Get(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp
new file mode 100644
index 0000000..dd1a824
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/Pneumatics.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Pneumatics::Pneumatics() : frc::Subsystem("Pneumatics") {
+ AddChild("Pressure Sensor", m_pressureSensor);
+}
+
+void Pneumatics::InitDefaultCommand() {
+ // No default command
+}
+
+void Pneumatics::Start() {
+#ifndef SIMULATION
+ m_compressor.Start();
+#endif
+}
+
+bool Pneumatics::IsPressurized() {
+#ifndef SIMULATION
+ return kMaxPressure <= m_pressureSensor.GetVoltage();
+#else
+ return true; // NOTE: Simulation always has full pressure
+#endif
+}
+
+void Pneumatics::WritePressure() {
+ frc::SmartDashboard::PutNumber("Pressure", m_pressureSensor.GetVoltage());
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Shooter.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Shooter.cpp
new file mode 100644
index 0000000..80d7ac3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Shooter.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/Shooter.h"
+
+Shooter::Shooter() : Subsystem("Shooter") {
+ // Put everything to the LiveWindow for testing.
+ AddChild("Hot Goal Sensor", m_hotGoalSensor);
+ AddChild("Piston1 Reed Switch Front ", m_piston1ReedSwitchFront);
+ AddChild("Piston1 Reed Switch Back ", m_piston1ReedSwitchBack);
+ AddChild("Latch Piston", m_latchPiston);
+}
+
+void Shooter::InitDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // SetDefaultCommand(new MySpecialCommand());
+}
+
+void Shooter::ExtendBoth() {
+ m_piston1.Set(frc::DoubleSolenoid::kForward);
+ m_piston2.Set(frc::DoubleSolenoid::kForward);
+}
+
+void Shooter::RetractBoth() {
+ m_piston1.Set(frc::DoubleSolenoid::kReverse);
+ m_piston2.Set(frc::DoubleSolenoid::kReverse);
+}
+
+void Shooter::Extend1() { m_piston1.Set(frc::DoubleSolenoid::kForward); }
+
+void Shooter::Retract1() { m_piston1.Set(frc::DoubleSolenoid::kReverse); }
+
+void Shooter::Extend2() { m_piston2.Set(frc::DoubleSolenoid::kReverse); }
+
+void Shooter::Retract2() { m_piston2.Set(frc::DoubleSolenoid::kForward); }
+
+void Shooter::Off1() { m_piston1.Set(frc::DoubleSolenoid::kOff); }
+
+void Shooter::Off2() { m_piston2.Set(frc::DoubleSolenoid::kOff); }
+
+void Shooter::Unlatch() { m_latchPiston.Set(true); }
+
+void Shooter::Latch() { m_latchPiston.Set(false); }
+
+void Shooter::ToggleLatchPosition() { m_latchPiston.Set(!m_latchPiston.Get()); }
+
+bool Shooter::Piston1IsExtended() { return !m_piston1ReedSwitchFront.Get(); }
+
+bool Shooter::Piston1IsRetracted() { return !m_piston1ReedSwitchBack.Get(); }
+
+void Shooter::OffBoth() {
+ m_piston1.Set(frc::DoubleSolenoid::kOff);
+ m_piston2.Set(frc::DoubleSolenoid::kOff);
+}
+
+bool Shooter::GoalIsHot() { return m_hotGoalSensor.Get(); }
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/triggers/DoubleButton.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/triggers/DoubleButton.cpp
new file mode 100644
index 0000000..f2f7f64
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/triggers/DoubleButton.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "triggers/DoubleButton.h"
+
+#include <frc/Joystick.h>
+
+DoubleButton::DoubleButton(frc::Joystick* joy, int button1, int button2)
+ : m_joy(*joy) {
+ m_button1 = button1;
+ m_button2 = button2;
+}
+
+bool DoubleButton::Get() {
+ return m_joy.GetRawButton(m_button1) && m_joy.GetRawButton(m_button2);
+}
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/OI.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/OI.h
new file mode 100644
index 0000000..2df4395
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/OI.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Joystick.h>
+#include <frc/buttons/JoystickButton.h>
+
+#include "triggers/DoubleButton.h"
+
+class OI {
+ public:
+ OI();
+ frc::Joystick& GetJoystick();
+
+ private:
+ frc::Joystick m_joystick{0};
+
+ frc::JoystickButton m_l1{&m_joystick, 11};
+ frc::JoystickButton m_l2{&m_joystick, 9};
+ frc::JoystickButton m_r1{&m_joystick, 12};
+ frc::JoystickButton m_r2{&m_joystick, 10};
+
+ DoubleButton m_sticks{&m_joystick, 2, 3};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h
new file mode 100644
index 0000000..b24085d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/TimedRobot.h>
+#include <frc/commands/Command.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+#include "OI.h"
+#include "commands/DriveAndShootAutonomous.h"
+#include "commands/DriveForward.h"
+#include "subsystems/Collector.h"
+#include "subsystems/DriveTrain.h"
+#include "subsystems/Pivot.h"
+#include "subsystems/Pneumatics.h"
+#include "subsystems/Shooter.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ static DriveTrain drivetrain;
+ static Pivot pivot;
+ static Collector collector;
+ static Shooter shooter;
+ static Pneumatics pneumatics;
+ static OI oi;
+
+ private:
+ frc::Command* m_autonomousCommand = nullptr;
+ DriveAndShootAutonomous m_driveAndShootAuto;
+ DriveForward m_driveForwardAuto;
+ frc::SendableChooser<frc::Command*> m_autoChooser;
+
+ void RobotInit() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+
+ void Log();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CheckForHotGoal.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CheckForHotGoal.h
new file mode 100644
index 0000000..9f42461
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CheckForHotGoal.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * This command looks for the hot goal and waits until it's detected or timed
+ * out. The timeout is because it's better to shoot and get some autonomous
+ * points than get none. When called sequentially, this command will block until
+ * the hot goal is detected or until it is timed out.
+ */
+class CheckForHotGoal : public frc::Command {
+ public:
+ explicit CheckForHotGoal(double time);
+ bool IsFinished() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CloseClaw.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CloseClaw.h
new file mode 100644
index 0000000..60f12a9
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CloseClaw.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/InstantCommand.h>
+
+/**
+ * Close the claw.
+ *
+ * NOTE: It doesn't wait for the claw to close since there is no sensor to
+ * detect that.
+ */
+class CloseClaw : public frc::InstantCommand {
+ public:
+ CloseClaw();
+ void Initialize() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Collect.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Collect.h
new file mode 100644
index 0000000..c2965c2
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Collect.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+/**
+ * Get the robot set to collect balls.
+ */
+class Collect : public frc::CommandGroup {
+ public:
+ Collect();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveAndShootAutonomous.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveAndShootAutonomous.h
new file mode 100644
index 0000000..e143738
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveAndShootAutonomous.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+/**
+ * Drive over the line and then shoot the ball. If the hot goal is not detected,
+ * it will wait briefly.
+ */
+class DriveAndShootAutonomous : public frc::CommandGroup {
+ public:
+ DriveAndShootAutonomous();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveForward.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveForward.h
new file mode 100644
index 0000000..a89134e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveForward.h
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * This command drives the robot over a given distance with simple proportional
+ * control This command will drive a given distance limiting to a maximum speed.
+ */
+class DriveForward : public frc::Command {
+ public:
+ DriveForward();
+ explicit DriveForward(double dist);
+ DriveForward(double dist, double maxSpeed);
+ void Initialize() override;
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+
+ private:
+ double m_driveForwardSpeed;
+ double m_distance;
+ double m_error = 0;
+ static constexpr double kTolerance = 0.1;
+ static constexpr double kP = -1.0 / 5.0;
+
+ void init(double dist, double maxSpeed);
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveWithJoystick.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveWithJoystick.h
new file mode 100644
index 0000000..e6d9b0e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveWithJoystick.h
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * This command allows PS3 joystick to drive the robot. It is always running
+ * except when interrupted by another command.
+ */
+class DriveWithJoystick : public frc::Command {
+ public:
+ DriveWithJoystick();
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/ExtendShooter.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/ExtendShooter.h
new file mode 100644
index 0000000..634860e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/ExtendShooter.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/TimedCommand.h>
+
+/**
+ * Extend the shooter and then retract it after a second.
+ */
+class ExtendShooter : public frc::TimedCommand {
+ public:
+ ExtendShooter();
+ void Initialize() override;
+ void End() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/LowGoal.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/LowGoal.h
new file mode 100644
index 0000000..62237fe
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/LowGoal.h
@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+/**
+ * Spit the ball out into the low goal assuming that the robot is in front of
+ * it.
+ */
+class LowGoal : public frc::CommandGroup {
+ public:
+ LowGoal();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/OpenClaw.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/OpenClaw.h
new file mode 100644
index 0000000..0d78877
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/OpenClaw.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Opens the claw
+ */
+class OpenClaw : public frc::Command {
+ public:
+ OpenClaw();
+ void Initialize() override;
+ bool IsFinished() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetCollectionSpeed.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetCollectionSpeed.h
new file mode 100644
index 0000000..24bb4e3
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetCollectionSpeed.h
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/InstantCommand.h>
+
+/**
+ * This command sets the collector rollers spinning at the given speed. Since
+ * there is no sensor for detecting speed, it finishes immediately. As a result,
+ * the spinners may still be adjusting their speed.
+ */
+class SetCollectionSpeed : public frc::InstantCommand {
+ public:
+ explicit SetCollectionSpeed(double speed);
+ void Initialize() override;
+
+ private:
+ double m_speed;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetPivotSetpoint.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetPivotSetpoint.h
new file mode 100644
index 0000000..8f8f615
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetPivotSetpoint.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Moves the pivot to a given angle. This command finishes when it is within
+ * the tolerance, but leaves the PID loop running to maintain the position.
+ * Other commands using the pivot should make sure they disable PID!
+ */
+class SetPivotSetpoint : public frc::Command {
+ public:
+ explicit SetPivotSetpoint(double setpoint);
+ void Initialize() override;
+ bool IsFinished() override;
+
+ private:
+ double m_setpoint;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Shoot.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Shoot.h
new file mode 100644
index 0000000..242221c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Shoot.h
@@ -0,0 +1,18 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/CommandGroup.h>
+
+/**
+ * Shoot the ball at the current angle.
+ */
+class Shoot : public frc::CommandGroup {
+ public:
+ Shoot();
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForBall.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForBall.h
new file mode 100644
index 0000000..b6d8d99
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForBall.h
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Wait until the collector senses that it has the ball. This command does
+ * nothing and is intended to be used in command groups to wait for this
+ * condition.
+ */
+class WaitForBall : public frc::Command {
+ public:
+ WaitForBall();
+ bool IsFinished() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForPressure.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForPressure.h
new file mode 100644
index 0000000..49e1f17
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForPressure.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+/**
+ * Wait until the pneumatics are fully pressurized. This command does nothing
+ * and is intended to be used in command groups to wait for this condition.
+ */
+class WaitForPressure : public frc::Command {
+ public:
+ WaitForPressure();
+ bool IsFinished() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h
new file mode 100644
index 0000000..3e237fa
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/DigitalInput.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/Solenoid.h>
+#include <frc/commands/Subsystem.h>
+
+/**
+ * The Collector subsystem has one motor for the rollers, a limit switch for
+ * ball
+ * detection, a piston for opening and closing the claw, and a reed switch to
+ * check if the piston is open.
+ */
+class Collector : public frc::Subsystem {
+ public:
+ // Constants for some useful speeds
+ static constexpr double kForward = 1;
+ static constexpr double kStop = 0;
+ static constexpr double kReverse = -1;
+
+ Collector();
+
+ /**
+ * NOTE: The current simulation model uses the the lower part of the
+ * claw
+ * since the limit switch wasn't exported. At some point, this will be
+ * updated.
+ *
+ * @return Whether or not the robot has the ball.
+ */
+ bool HasBall();
+
+ /**
+ * @param speed The speed to spin the rollers.
+ */
+ void SetSpeed(double speed);
+
+ /**
+ * Stop the rollers from spinning
+ */
+ void Stop();
+
+ /**
+ * @return Whether or not the claw is open.
+ */
+ bool IsOpen();
+
+ /**
+ * Open the claw up. (For shooting)
+ */
+ void Open();
+
+ /**
+ * Close the claw. (For collecting and driving)
+ */
+ void Close();
+
+ /**
+ * No default command.
+ */
+ void InitDefaultCommand() override;
+
+ private:
+ // Subsystem devices
+ frc::PWMVictorSPX m_rollerMotor{6};
+ frc::DigitalInput m_ballDetector{10};
+ frc::Solenoid m_piston{1};
+ frc::DigitalInput m_openDetector{6};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/DriveTrain.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/DriveTrain.h
new file mode 100644
index 0000000..0afa3eb
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/DriveTrain.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SpeedControllerGroup.h>
+#include <frc/commands/Subsystem.h>
+#include <frc/drive/DifferentialDrive.h>
+
+namespace frc {
+class Joystick;
+} // namespace frc
+
+/**
+ * The DriveTrain subsystem controls the robot's chassis and reads in
+ * information about it's speed and position.
+ */
+class DriveTrain : public frc::Subsystem {
+ public:
+ DriveTrain();
+
+ /**
+ * When other commands aren't using the drivetrain, allow tank drive
+ * with
+ * the joystick.
+ */
+ void InitDefaultCommand();
+
+ /**
+ * @param leftAxis Left sides value
+ * @param rightAxis Right sides value
+ */
+ void TankDrive(double leftAxis, double rightAxis);
+
+ /**
+ * Stop the drivetrain from moving.
+ */
+ void Stop();
+
+ /**
+ * @return The encoder getting the distance and speed of left side of
+ * the drivetrain.
+ */
+ frc::Encoder& GetLeftEncoder();
+
+ /**
+ * @return The encoder getting the distance and speed of right side of
+ * the drivetrain.
+ */
+ frc::Encoder& GetRightEncoder();
+
+ /**
+ * @return The current angle of the drivetrain.
+ */
+ double GetAngle();
+
+ private:
+ // Subsystem devices
+ frc::PWMVictorSPX m_frontLeftCIM{1};
+ frc::PWMVictorSPX m_rearLeftCIM{2};
+ frc::SpeedControllerGroup m_leftCIMs{m_frontLeftCIM, m_rearLeftCIM};
+
+ frc::PWMVictorSPX m_frontRightCIM{3};
+ frc::PWMVictorSPX m_rearRightCIM{4};
+ frc::SpeedControllerGroup m_rightCIMs{m_frontRightCIM, m_rearRightCIM};
+
+ frc::DifferentialDrive m_robotDrive{m_leftCIMs, m_rightCIMs};
+
+ frc::Encoder m_rightEncoder{1, 2, true, frc::Encoder::k4X};
+ frc::Encoder m_leftEncoder{3, 4, false, frc::Encoder::k4X};
+ frc::AnalogGyro m_gyro{0};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pivot.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pivot.h
new file mode 100644
index 0000000..f9a3c25
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pivot.h
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogPotentiometer.h>
+#include <frc/DigitalInput.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/commands/PIDSubsystem.h>
+
+/**
+ * The Pivot subsystem contains the Van-door motor and the pot for PID control
+ * of angle of the pivot and claw.
+ */
+class Pivot : public frc::PIDSubsystem {
+ public:
+ // Constants for some useful angles
+ static constexpr double kCollect = 105;
+ static constexpr double kLowGoal = 90;
+ static constexpr double kShoot = 45;
+ static constexpr double kShootNear = 30;
+
+ Pivot();
+
+ /**
+ * No default command, if PID is enabled, the current setpoint will be
+ * maintained.
+ */
+ void InitDefaultCommand() override {}
+
+ /**
+ * @return The angle read in by the potentiometer
+ */
+ double ReturnPIDInput() override;
+
+ /**
+ * Set the motor speed based off of the PID output
+ */
+ void UsePIDOutput(double output) override;
+
+ /**
+ * @return If the pivot is at its upper limit.
+ */
+ bool IsAtUpperLimit();
+
+ /**
+ * @return If the pivot is at its lower limit.
+ */
+ bool IsAtLowerLimit();
+
+ /**
+ * @return The current angle of the pivot.
+ */
+ double GetAngle();
+
+ private:
+ // Subsystem devices
+
+ // Sensors for measuring the position of the pivot
+ frc::DigitalInput m_upperLimitSwitch{13};
+ frc::DigitalInput m_lowerLimitSwitch{12};
+
+ /* 0 degrees is vertical facing up.
+ * Angle increases the more forward the pivot goes.
+ */
+ frc::AnalogPotentiometer m_pot{1};
+
+ // Motor to move the pivot
+ frc::PWMVictorSPX m_motor{5};
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h
new file mode 100644
index 0000000..d321895
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h
@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogInput.h>
+#include <frc/Compressor.h>
+#include <frc/commands/Subsystem.h>
+
+/**
+ * The Pneumatics subsystem contains the compressor and a pressure sensor.
+ *
+ * NOTE: The simulator currently doesn't support the compressor or pressure
+ * sensors.
+ */
+class Pneumatics : public frc::Subsystem {
+ public:
+ Pneumatics();
+
+ /**
+ * No default command
+ */
+ void InitDefaultCommand() override;
+
+ /**
+ * Start the compressor going. The compressor automatically starts and
+ * stops as it goes above and below maximum pressure.
+ */
+ void Start();
+
+ /**
+ * @return Whether or not the system is fully pressurized.
+ */
+ bool IsPressurized();
+
+ /**
+ * Puts the pressure on the SmartDashboard.
+ */
+ void WritePressure();
+
+ private:
+ frc::AnalogInput m_pressureSensor{3};
+
+#ifndef SIMULATION
+ frc::Compressor m_compressor{1}; // TODO: (1, 14, 1, 8);
+#endif
+
+ static constexpr double kMaxPressure = 2.55;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h
new file mode 100644
index 0000000..43f111a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h
@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/DigitalInput.h>
+#include <frc/DoubleSolenoid.h>
+#include <frc/Solenoid.h>
+#include <frc/commands/Subsystem.h>
+
+/**
+ * The Shooter subsystem handles shooting. The mechanism for shooting is
+ * slightly complicated because it has to pneumatic cylinders for shooting, and
+ * a third latch to allow the pressure to partially build up and reduce the
+ * effect of the airflow. For shorter shots, when full power isn't needed, only
+ * one cylinder fires.
+ *
+ * NOTE: Simulation currently approximates this as as single pneumatic cylinder
+ * and ignores the latch.
+ */
+class Shooter : public frc::Subsystem {
+ public:
+ Shooter();
+ void InitDefaultCommand() override;
+
+ /**
+ * Extend both solenoids to shoot.
+ */
+ void ExtendBoth();
+
+ /**
+ * Retract both solenoids to prepare to shoot.
+ */
+ void RetractBoth();
+
+ /**
+ * Extend solenoid 1 to shoot.
+ */
+ void Extend1();
+
+ /**
+ * Retract solenoid 1 to prepare to shoot.
+ */
+ void Retract1();
+
+ /**
+ * Extend solenoid 2 to shoot.
+ */
+ void Extend2();
+
+ /**
+ * Retract solenoid 2 to prepare to shoot.
+ */
+ void Retract2();
+
+ /**
+ * Turns off the piston1 double solenoid. This won't actuate anything
+ * because double solenoids preserve their state when turned off. This
+ * should be called in order to reduce the amount of time that the coils
+ * are
+ * powered.
+ */
+ void Off1();
+
+ /**
+ * Turns off the piston1 double solenoid. This won't actuate anything
+ * because double solenoids preserve their state when turned off. This
+ * should be called in order to reduce the amount of time that the coils
+ * are
+ * powered.
+ */
+ void Off2();
+
+ /**
+ * Release the latch so that we can shoot
+ */
+ void Unlatch();
+
+ /**
+ * Latch so that pressure can build up and we aren't limited by air
+ * flow.
+ */
+ void Latch();
+
+ /**
+ * Toggles the latch postions
+ */
+ void ToggleLatchPosition();
+
+ /**
+ * @return Whether or not piston 1 is fully extended.
+ */
+ bool Piston1IsExtended();
+
+ /**
+ * @return Whether or not piston 1 is fully retracted.
+ */
+ bool Piston1IsRetracted();
+
+ /**
+ * Turns off all double solenoids. Double solenoids hold their position
+ * when
+ * they are turned off. We should turn them off whenever possible to
+ * extend
+ * the life of the coils
+ */
+ void OffBoth();
+
+ /**
+ * @return Whether or not the goal is hot as read by the banner sensor
+ */
+ bool GoalIsHot();
+
+ private:
+ // Devices
+ frc::DoubleSolenoid m_piston1{3, 4};
+ frc::DoubleSolenoid m_piston2{5, 6};
+ frc::Solenoid m_latchPiston{1, 2};
+ frc::DigitalInput m_piston1ReedSwitchFront{9};
+ frc::DigitalInput m_piston1ReedSwitchBack{11};
+ frc::DigitalInput m_hotGoalSensor{
+ 7}; // NOTE: Currently ignored in simulation
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/include/triggers/DoubleButton.h b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/triggers/DoubleButton.h
new file mode 100644
index 0000000..562154b
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/include/triggers/DoubleButton.h
@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/buttons/Trigger.h>
+
+namespace frc {
+class Joystick;
+} // namespace frc
+
+class DoubleButton : public frc::Trigger {
+ public:
+ DoubleButton(frc::Joystick* joy, int button1, int button2);
+
+ bool Get();
+
+ private:
+ frc::Joystick& m_joy;
+ int m_button1;
+ int m_button2;
+};
diff --git a/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
new file mode 100644
index 0000000..e01a04f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <array>
+
+#include <frc/AnalogInput.h>
+#include <frc/Joystick.h>
+#include <frc/PIDController.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * This is a sample program to demonstrate how to use a soft potentiometer and a
+ * PID Controller to reach and maintain position setpoints on an elevator
+ * mechanism.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override { m_pidController.SetInputRange(0, 5); }
+
+ void TeleopInit() override { m_pidController.Enable(); }
+
+ void TeleopPeriodic() override {
+ // When the button is pressed once, the selected elevator setpoint is
+ // incremented.
+ bool currentButtonValue = m_joystick.GetTrigger();
+ if (currentButtonValue && !m_previousButtonValue) {
+ // Index of the elevator setpoint wraps around
+ m_index = (m_index + 1) % (sizeof(kSetPoints) / 8);
+ }
+ m_previousButtonValue = currentButtonValue;
+
+ m_pidController.SetSetpoint(kSetPoints[m_index]);
+ }
+
+ private:
+ static constexpr int kPotChannel = 1;
+ static constexpr int kMotorChannel = 7;
+ static constexpr int kJoystickChannel = 0;
+
+ // Bottom, middle, and top elevator setpoints
+ static constexpr std::array<double, 3> kSetPoints = {{1.0, 2.6, 4.3}};
+
+ /* Proportional, integral, and derivative speed constants; motor inverted.
+ *
+ * DANGER: When tuning PID constants, high/inappropriate values for pGain,
+ * iGain, and dGain may cause dangerous, uncontrollable, or undesired
+ * behavior!
+ *
+ * These may need to be positive for a non-inverted motor.
+ */
+ static constexpr double kP = -5.0;
+ static constexpr double kI = -0.02;
+ static constexpr double kD = -2.0;
+
+ int m_index = 0;
+ bool m_previousButtonValue = false;
+
+ frc::AnalogInput m_potentiometer{kPotChannel};
+ frc::Joystick m_joystick{kJoystickChannel};
+ frc::PWMVictorSPX m_elevatorMotor{kMotorChannel};
+
+ /* Potentiometer (AnalogInput) and elevatorMotor (Victor) can be used as a
+ * PIDSource and PIDOutput respectively.
+ */
+ frc::PIDController m_pidController{kP, kI, kD, m_potentiometer,
+ m_elevatorMotor};
+};
+
+constexpr std::array<double, 3> Robot::kSetPoints;
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/QuickVision/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/QuickVision/cpp/Robot.cpp
new file mode 100644
index 0000000..1ad76de
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/QuickVision/cpp/Robot.cpp
@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <cameraserver/CameraServer.h>
+#include <frc/TimedRobot.h>
+#include <wpi/raw_ostream.h>
+
+/**
+ * Uses the CameraServer class to automatically capture video from a USB webcam
+ * and send it to the FRC dashboard without doing any vision processing. This is
+ * the easiest way to get camera images to the dashboard. Just add this to the
+ * RobotInit() method in your program.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override {
+#if defined(__linux__)
+ frc::CameraServer::GetInstance()->StartAutomaticCapture();
+#else
+ wpi::errs() << "Vision only available on Linux.\n";
+ wpi::errs().flush();
+#endif
+ }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Relay/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Relay/cpp/Robot.cpp
new file mode 100644
index 0000000..8225b1c
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Relay/cpp/Robot.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/Joystick.h>
+#include <frc/Relay.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * This is a sample program which uses joystick buttons to control a relay.
+ *
+ * A Relay (generally a spike) has two outputs, each of which can be at either
+ * 0V or 12V and so can be used for actions such as turning a motor off, full
+ * forwards, or full reverse, and is generally used on the compressor.
+ *
+ * This program uses two buttons on a joystick and each button corresponds to
+ * one output; pressing the button sets the output to 12V and releasing sets it
+ * to 0V.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void TeleopPeriodic() override {
+ /* Retrieve the button values. GetRawButton() will return true if the button
+ * is pressed and false if not.
+ */
+ bool forward = m_stick.GetRawButton(kRelayForwardButton);
+ bool reverse = m_stick.GetRawButton(kRelayReverseButton);
+
+ /* Depending on the button values, we want to use one of kOn, kOff,
+ * kForward, or kReverse.
+ *
+ * kOn sets both outputs to 12V, kOff sets both to 0V.
+ * kForward sets forward to 12V and reverse to 0V.
+ * kReverse sets reverse to 12V and forward to 0V.
+ */
+ if (forward && reverse) {
+ m_relay.Set(frc::Relay::kOn);
+ } else if (forward) {
+ m_relay.Set(frc::Relay::kForward);
+ } else if (reverse) {
+ m_relay.Set(frc::Relay::kReverse);
+ } else {
+ m_relay.Set(frc::Relay::kOff);
+ }
+ }
+
+ private:
+ frc::Joystick m_stick{0};
+ frc::Relay m_relay{0};
+
+ static constexpr int kRelayForwardButton = 1;
+ static constexpr int kRelayReverseButton = 2;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp
new file mode 100644
index 0000000..3447504
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp
@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogPotentiometer.h>
+#include <frc/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/shuffleboard/Shuffleboard.h>
+#include <frc/shuffleboard/ShuffleboardLayout.h>
+#include <frc/shuffleboard/ShuffleboardTab.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
+
+/**
+ * This sample program provides an example for ShuffleBoard, an alternative
+ * to SmartDashboard for displaying values and properties of different robot
+ * parts.
+ *
+ * ShuffleBoard can use pre-programmed widgets to display various values, such
+ * as Boolean Boxes, Sliders, Graphs, and more. In addition, they can display
+ * things in various Tabs.
+ *
+ * For more information on how to create personal layouts and more in
+ * ShuffleBoard, feel free to reference the official FIRST WPILib documentation
+ * online.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override {
+ // Add a widget titled 'Max Speed' with a number slider.
+ m_maxSpeed = frc::Shuffleboard::GetTab("Configuration")
+ .Add("Max Speed", 1)
+ .WithWidget("Number Slider")
+ .GetEntry();
+
+ // Create a 'DriveBase' tab and add the drivetrain object to it.
+ frc::ShuffleboardTab& driveBaseTab = frc::Shuffleboard::GetTab("DriveBase");
+ driveBaseTab.Add("TankDrive", m_robotDrive);
+
+ // Put encoders in a list layout.
+ frc::ShuffleboardLayout& encoders =
+ driveBaseTab.GetLayout("List Layout", "Encoders")
+ .WithPosition(0, 0)
+ .WithSize(2, 2);
+ encoders.Add("Left Encoder", m_leftEncoder);
+ encoders.Add("Right Encoder", m_rightEncoder);
+
+ // Create a 'Elevator' tab and add the potentiometer and elevator motor to
+ // it.
+ frc::ShuffleboardTab& elevatorTab = frc::Shuffleboard::GetTab("Elevator");
+ elevatorTab.Add("Motor", m_elevatorMotor);
+ elevatorTab.Add("Potentiometer", m_ElevatorPot);
+ }
+
+ void AutonomousInit() override {
+ // Update the Max Output for the drivetrain.
+ m_robotDrive.SetMaxOutput(m_maxSpeed.GetDouble(1.0));
+ }
+
+ private:
+ frc::PWMVictorSPX m_left{0};
+ frc::PWMVictorSPX m_right{1};
+ frc::PWMVictorSPX m_elevatorMotor{2};
+
+ frc::DifferentialDrive m_robotDrive{m_left, m_right};
+
+ frc::Joystick m_stick{0};
+
+ frc::Encoder m_leftEncoder{0, 1};
+ frc::Encoder m_rightEncoder{2, 3};
+ frc::AnalogPotentiometer m_ElevatorPot{0};
+
+ nt::NetworkTableEntry m_maxSpeed;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp
new file mode 100644
index 0000000..523a227
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp
@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/DoubleSolenoid.h>
+#include <frc/Joystick.h>
+#include <frc/Solenoid.h>
+#include <frc/TimedRobot.h>
+
+/**
+ * This is a sample program showing the use of the solenoid classes during
+ * operator control.
+ *
+ * Three buttons from a joystick will be used to control two solenoids: One
+ * button to control the position of a single solenoid and the other two buttons
+ * to control a double solenoid.
+ *
+ * Single solenoids can either be on or off, such that the air diverted through
+ * them goes through either one channel or the other.
+ *
+ * Double solenoids have three states: Off, Forward, and Reverse. Forward and
+ * Reverse divert the air through the two channels and correspond to the on and
+ * off of a single solenoid, but a double solenoid can also be "off", where both
+ * channels are diverted to exhaust such that there is no pressure in either
+ * channel.
+ *
+ * Additionally, double solenoids take up two channels on your PCM whereas
+ * single solenoids only take a single channel.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ void TeleopPeriodic() override {
+ /* The output of GetRawButton is true/false depending on whether the button
+ * is pressed; Set takes a boolean for for whether to use the default
+ * (false) channel or the other (true).
+ */
+ m_solenoid.Set(m_stick.GetRawButton(kSolenoidButton));
+
+ /* In order to set the double solenoid, we will say that if neither button
+ * is pressed, it is off, if just one button is pressed, set the solenoid to
+ * correspond to that button, and if both are pressed, set the solenoid to
+ * Forwards.
+ */
+ if (m_stick.GetRawButton(kDoubleSolenoidForward)) {
+ m_doubleSolenoid.Set(frc::DoubleSolenoid::kForward);
+ } else if (m_stick.GetRawButton(kDoubleSolenoidReverse)) {
+ m_doubleSolenoid.Set(frc::DoubleSolenoid::kReverse);
+ } else {
+ m_doubleSolenoid.Set(frc::DoubleSolenoid::kOff);
+ }
+ }
+
+ private:
+ frc::Joystick m_stick{0};
+
+ // Solenoid corresponds to a single solenoid.
+ frc::Solenoid m_solenoid{0};
+
+ // DoubleSolenoid corresponds to a double solenoid.
+ frc::DoubleSolenoid m_doubleSolenoid{1, 2};
+
+ static constexpr int kSolenoidButton = 1;
+ static constexpr int kDoubleSolenoidForward = 2;
+ static constexpr int kDoubleSolenoidReverse = 3;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
new file mode 100644
index 0000000..2cc2131
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogInput.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a sample program demonstrating how to use an ultrasonic sensor and
+ * proportional control to maintain a set distance from an object.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ /**
+ * Tells the robot to drive to a set distance (in inches) from an object using
+ * proportional control.
+ */
+ void TeleopPeriodic() override {
+ // Sensor returns a value from 0-4095 that is scaled to inches
+ double currentDistance = m_ultrasonic.GetValue() * kValueToInches;
+ // Convert distance error to a motor speed
+ double currentSpeed = (kHoldDistance - currentDistance) * kP;
+ // Drive robot
+ m_robotDrive.ArcadeDrive(currentSpeed, 0);
+ }
+
+ private:
+ // Distance in inches the robot wants to stay from an object
+ static constexpr int kHoldDistance = 12;
+
+ // Factor to convert sensor values to a distance in inches
+ static constexpr double kValueToInches = 0.125;
+
+ // Proportional speed constant
+ static constexpr double kP = 0.05;
+
+ static constexpr int kLeftMotorPort = 0;
+ static constexpr int kRightMotorPort = 1;
+ static constexpr int kUltrasonicPort = 0;
+
+ frc::AnalogInput m_ultrasonic{kUltrasonicPort};
+
+ frc::PWMVictorSPX m_left{kLeftMotorPort};
+ frc::PWMVictorSPX m_right{kRightMotorPort};
+ frc::DifferentialDrive m_robotDrive{m_left, m_right};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
new file mode 100644
index 0000000..67aec78
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/AnalogInput.h>
+#include <frc/PIDController.h>
+#include <frc/PIDOutput.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/DifferentialDrive.h>
+
+/**
+ * This is a sample program demonstrating how to use an ultrasonic sensor and
+ * proportional control to maintain a set distance from an object.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+ /**
+ * Drives the robot a set distance from an object using PID control and the
+ * ultrasonic sensor.
+ */
+ void TeleopInit() override {
+ // Set expected range to 0-24 inches; e.g. at 24 inches from object go full
+ // forward, at 0 inches from object go full backward.
+ m_pidController.SetInputRange(0, 24 * kValueToInches);
+
+ // Set setpoint of the PID Controller
+ m_pidController.SetSetpoint(kHoldDistance * kValueToInches);
+
+ // Begin PID control
+ m_pidController.Enable();
+ }
+
+ private:
+ // Internal class to write to robot drive using a PIDOutput
+ class MyPIDOutput : public frc::PIDOutput {
+ public:
+ explicit MyPIDOutput(frc::DifferentialDrive& r) : m_rd(r) {
+ m_rd.SetSafetyEnabled(false);
+ }
+
+ void PIDWrite(double output) override {
+ // Write to robot drive by reference
+ m_rd.ArcadeDrive(output, 0);
+ }
+
+ private:
+ frc::DifferentialDrive& m_rd;
+ };
+
+ // Distance in inches the robot wants to stay from an object
+ static constexpr int kHoldDistance = 12;
+
+ // Factor to convert sensor values to a distance in inches
+ static constexpr double kValueToInches = 0.125;
+
+ // proportional speed constant
+ static constexpr double kP = 7.0;
+
+ // integral speed constant
+ static constexpr double kI = 0.018;
+
+ // derivative speed constant
+ static constexpr double kD = 1.5;
+
+ static constexpr int kLeftMotorPort = 0;
+ static constexpr int kRightMotorPort = 1;
+ static constexpr int kUltrasonicPort = 0;
+
+ frc::AnalogInput m_ultrasonic{kUltrasonicPort};
+
+ frc::PWMVictorSPX m_left{kLeftMotorPort};
+ frc::PWMVictorSPX m_right{kRightMotorPort};
+ frc::DifferentialDrive m_robotDrive{m_left, m_right};
+ MyPIDOutput m_pidOutput{m_robotDrive};
+
+ frc::PIDController m_pidController{kP, kI, kD, m_ultrasonic, m_pidOutput};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json
new file mode 100644
index 0000000..8859134
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -0,0 +1,247 @@
+[
+ {
+ "name": "Motor Controller",
+ "description": "Demonstrate controlling a single motor with a Joystick.",
+ "tags": [
+ "Robot and Motor",
+ "Actuators",
+ "Joystick",
+ "Complete List"
+ ],
+ "foldername": "MotorControl",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Motor Control With Encoder",
+ "description": "Demonstrate controlling a single motor with a Joystick and displaying the net movement of the motor using an encoder.",
+ "tags": [
+ "Robot and Motor",
+ "Digital",
+ "Sensors",
+ "Actuators",
+ "Joystick",
+ "Complete List"
+ ],
+ "foldername": "MotorControlEncoder",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Relay",
+ "description": "Demonstrate controlling a Relay from Joystick buttons.",
+ "tags": [
+ "Actuators",
+ "Joystick",
+ "Complete List"
+ ],
+ "foldername": "Relay",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "PDP CAN Monitoring",
+ "description": "Demonstrate using CAN to monitor the voltage, current, and temperature in the Power Distribution Panel.",
+ "tags": [
+ "Complete List",
+ "CAN",
+ "Sensors"
+ ],
+ "foldername": "CANPDP",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Solenoids",
+ "description": "Demonstrate controlling a single and double solenoid from Joystick buttons.",
+ "tags": [
+ "Actuators",
+ "Joystick",
+ "Pneumatics",
+ "Complete List"
+ ],
+ "foldername": "Solenoid",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Encoder",
+ "description": "Demonstrate displaying the value of a quadrature encoder on the SmartDashboard.",
+ "tags": [
+ "Complete List",
+ "Digital",
+ "Sensors"
+ ],
+ "foldername": "Encoder",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Arcade Drive",
+ "description": "An example program which demonstrates the use of Arcade Drive with the DifferentialDrive class",
+ "tags": [
+ "Getting Started with C++",
+ "Robot and Motor",
+ "Joystick",
+ "Complete List"
+ ],
+ "foldername": "ArcadeDrive",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Mecanum Drive",
+ "description": "An example program which demonstrates the use of Mecanum Drive with the MecanumDrive class",
+ "tags": [
+ "Getting Started with C++",
+ "Robot and Motor",
+ "Joystick",
+ "Complete List"
+ ],
+ "foldername": "MecanumDrive",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Ultrasonic",
+ "description": "Demonstrate maintaining a set distance using an ultrasonic sensor.",
+ "tags": [
+ "Robot and Motor",
+ "Complete List",
+ "Sensors",
+ "Analog"
+ ],
+ "foldername": "Ultrasonic",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "UltrasonicPID",
+ "description": "Demonstrate maintaining a set distance using an ultrasonic sensor and PID control.",
+ "tags": [
+ "Robot and Motor",
+ "Complete List",
+ "Sensors",
+ "Analog"
+ ],
+ "foldername": "UltrasonicPID",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Gyro",
+ "description": "An example program showing how to drive straight with using a gyro sensor.",
+ "tags": [
+ "Robot and Motor",
+ "Complete List",
+ "Sensors",
+ "Analog",
+ "Joystick"
+ ],
+ "foldername": "Gyro",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Gyro Mecanum",
+ "description": "An example program showing how to perform mecanum drive with field oriented controls.",
+ "tags": [
+ "Robot and Motor",
+ "Complete List",
+ "Sensors",
+ "Analog",
+ "Joysitck"
+ ],
+ "foldername": "GyroMecanum",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "HID Rumble",
+ "description": "An example program showing how to make human interface devices rumble.",
+ "tags": [
+ "Joystick"
+ ],
+ "foldername": "HidRumble",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "PotentiometerPID",
+ "description": "An example to demonstrate the use of a potentiometer and PID control to reach elevator position setpoints.",
+ "tags": [
+ "Joystick",
+ "Actuators",
+ "Complete List",
+ "Sensors",
+ "Analog"
+ ],
+ "foldername": "PotentiometerPID",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Getting Started",
+ "description": "An example program which demonstrates the simplest autonomous and teleoperated routines.",
+ "tags": [
+ "Getting Started with C++",
+ "Complete List"
+ ],
+ "foldername": "GettingStarted",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Simple Vision",
+ "description": "The minimal program to acquire images from an attached USB camera on the robot and send them to the dashboard.",
+ "tags": [
+ "Vision",
+ "Complete List"
+ ],
+ "foldername": "QuickVision",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Intermediate Vision",
+ "description": "An example program that acquires images from an attached USB camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display.",
+ "tags": [
+ "Vision",
+ "Complete List"
+ ],
+ "foldername": "IntermediateVision",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Axis Camera Sample",
+ "description": "An example program that acquires images from an Axis network camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display. This demonstrates the use of the AxisCamera class.",
+ "tags": [
+ "Vision",
+ "Complete List"
+ ],
+ "foldername": "AxisCameraSample",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "GearsBot",
+ "description": "A fully functional example CommandBased program for WPIs GearsBot robot. This code can run on your computer if it supports simulation.",
+ "tags": [
+ "CommandBased Robot",
+ "Complete List"
+ ],
+ "foldername": "GearsBot",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "PacGoat",
+ "description": "A fully functional example CommandBased program for FRC Team 190's 2014 robot. This code can run on your computer if it supports simulation.",
+ "tags": [
+ "CommandBased Robot",
+ "Complete List"
+ ],
+ "foldername": "PacGoat",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "HAL",
+ "description": "A program created using the HAL exclusively. This example is for advanced users",
+ "tags": [
+ "HAL"
+ ],
+ "foldername": "HAL",
+ "gradlebase": "c"
+ },
+ {
+ "name": "ShuffleBoard",
+ "description": "An example program that uses ShuffleBoard with its Widgets and Tabs.",
+ "tags": [
+ "ShuffleBoard"
+ ],
+ "foldername": "ShuffleBoard",
+ "gradlebase": "cpp"
+ }
+]
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp
new file mode 100644
index 0000000..5974f1f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/OI.cpp
@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "OI.h"
+
+#include <frc/WPILib.h>
+
+OI::OI() {
+ // Process operator interface input here.
+}
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp
new file mode 100644
index 0000000..df2a9cc
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Scheduler.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+ExampleSubsystem Robot::m_subsystem;
+OI Robot::m_oi;
+
+void Robot::RobotInit() {
+ m_chooser.SetDefaultOption("Default Auto", &m_defaultAuto);
+ m_chooser.AddOption("My Auto", &m_myAuto);
+ frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {}
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString code to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons to
+ * the if-else structure below with additional strings & commands.
+ */
+void Robot::AutonomousInit() {
+ // std::string autoSelected = frc::SmartDashboard::GetString(
+ // "Auto Selector", "Default");
+ // if (autoSelected == "My Auto") {
+ // m_autonomousCommand = &m_myAuto;
+ // } else {
+ // m_autonomousCommand = &m_defaultAuto;
+ // }
+
+ m_autonomousCommand = m_chooser.GetSelected();
+
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Start();
+ }
+}
+
+void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TeleopInit() {
+ // This makes sure that the autonomous stops running when
+ // teleop starts running. If you want the autonomous to
+ // continue until interrupted by another command, remove
+ // this line or comment it out.
+ if (m_autonomousCommand != nullptr) {
+ m_autonomousCommand->Cancel();
+ m_autonomousCommand = nullptr;
+ }
+}
+
+void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
+
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp
new file mode 100644
index 0000000..fa83682
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/ExampleCommand.h"
+
+#include "Robot.h"
+
+ExampleCommand::ExampleCommand() {
+ // Use Requires() here to declare subsystem dependencies
+ Requires(&Robot::m_subsystem);
+}
+
+// Called just before this Command runs the first time
+void ExampleCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void ExampleCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool ExampleCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void ExampleCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void ExampleCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp
new file mode 100644
index 0000000..a8e9406
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/MyAutoCommand.cpp
@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "commands/MyAutoCommand.h"
+
+#include "Robot.h"
+
+MyAutoCommand::MyAutoCommand() {
+ // Use Requires() here to declare subsystem dependencies
+ Requires(&Robot::m_subsystem);
+}
+
+// Called just before this Command runs the first time
+void MyAutoCommand::Initialize() {}
+
+// Called repeatedly when this Command is scheduled to run
+void MyAutoCommand::Execute() {}
+
+// Make this return true when this Command no longer needs to run execute()
+bool MyAutoCommand::IsFinished() { return false; }
+
+// Called once after isFinished returns true
+void MyAutoCommand::End() {}
+
+// Called when another command which requires one or more of the same
+// subsystems is scheduled to run
+void MyAutoCommand::Interrupted() {}
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
new file mode 100644
index 0000000..cdde203
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 "subsystems/ExampleSubsystem.h"
+
+#include "RobotMap.h"
+
+ExampleSubsystem::ExampleSubsystem() : frc::Subsystem("ExampleSubsystem") {}
+
+void ExampleSubsystem::InitDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // SetDefaultCommand(new MySpecialCommand());
+}
+
+// Put methods for controlling this subsystem
+// here. Call these from Commands.
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h
new file mode 100644
index 0000000..0b7713e
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/OI.h
@@ -0,0 +1,13 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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
+
+class OI {
+ public:
+ OI();
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h
new file mode 100644
index 0000000..7dd5093
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/TimedRobot.h>
+#include <frc/commands/Command.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+#include "OI.h"
+#include "commands/ExampleCommand.h"
+#include "commands/MyAutoCommand.h"
+#include "subsystems/ExampleSubsystem.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+ static ExampleSubsystem m_subsystem;
+ static OI m_oi;
+
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void DisabledInit() override;
+ void DisabledPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ // Have it null by default so that if testing teleop it
+ // doesn't have undefined behavior and potentially crash.
+ frc::Command* m_autonomousCommand = nullptr;
+ ExampleCommand m_defaultAuto;
+ MyAutoCommand m_myAuto;
+ frc::SendableChooser<frc::Command*> m_chooser;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h
new file mode 100644
index 0000000..dd78a21
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotMap.h
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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
+
+/**
+ * The RobotMap is a mapping from the ports sensors and actuators are wired into
+ * to a variable name. This provides flexibility changing wiring, makes checking
+ * the wiring easier and significantly reduces the number of magic numbers
+ * floating around.
+ */
+
+// For example to map the left and right motors, you could define the
+// following variables to use with your drivetrain subsystem.
+// constexpr int kLeftMotor = 1;
+// constexpr int kRightMotor = 2;
+
+// If you are using multiple modules, make sure to define both the port
+// number and the module. For example you with a rangefinder:
+// constexpr int kRangeFinderPort = 1;
+// constexpr int kRangeFinderModule = 1;
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h
new file mode 100644
index 0000000..1d11728
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+class ExampleCommand : public frc::Command {
+ public:
+ ExampleCommand();
+ void Initialize() override;
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+ void Interrupted() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h
new file mode 100644
index 0000000..ce25e76
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/MyAutoCommand.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Command.h>
+
+class MyAutoCommand : public frc::Command {
+ public:
+ MyAutoCommand();
+ void Initialize() override;
+ void Execute() override;
+ bool IsFinished() override;
+ void End() override;
+ void Interrupted() override;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
new file mode 100644
index 0000000..66bc329
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <frc/commands/Subsystem.h>
+
+class ExampleSubsystem : public frc::Subsystem {
+ public:
+ ExampleSubsystem();
+ void InitDefaultCommand() override;
+
+ private:
+ // It's desirable that everything possible under private except
+ // for methods that implement subsystem capabilities
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp
new file mode 100644
index 0000000..942fc9a
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/iterative/cpp/Robot.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <iostream>
+
+#include <frc/smartdashboard/SmartDashboard.h>
+
+void Robot::RobotInit() {
+ m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
+ m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
+ frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {}
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString line to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional comparisons to the
+ * if-else structure below with additional strings. If using the SendableChooser
+ * make sure to add them to the chooser code above as well.
+ */
+void Robot::AutonomousInit() {
+ m_autoSelected = m_chooser.GetSelected();
+ // m_autoSelected = SmartDashboard::GetString(
+ // "Auto Selector", kAutoNameDefault);
+ std::cout << "Auto selected: " << m_autoSelected << std::endl;
+
+ if (m_autoSelected == kAutoNameCustom) {
+ // Custom Auto goes here
+ } else {
+ // Default Auto goes here
+ }
+}
+
+void Robot::AutonomousPeriodic() {
+ if (m_autoSelected == kAutoNameCustom) {
+ // Custom Auto goes here
+ } else {
+ // Default Auto goes here
+ }
+}
+
+void Robot::TeleopInit() {}
+
+void Robot::TeleopPeriodic() {}
+
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
new file mode 100644
index 0000000..2c70b8f
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/iterative/include/Robot.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <string>
+
+#include <frc/IterativeRobot.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+class Robot : public frc::IterativeRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ frc::SendableChooser<std::string> m_chooser;
+ const std::string kAutoNameDefault = "Default";
+ const std::string kAutoNameCustom = "My Auto";
+ std::string m_autoSelected;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/sample/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/sample/cpp/Robot.cpp
new file mode 100644
index 0000000..06352ea
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/sample/cpp/Robot.cpp
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <iostream>
+
+#include <frc/Timer.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+
+Robot::Robot() {
+ // Note SmartDashboard is not initialized here, wait until RobotInit() to make
+ // SmartDashboard calls
+ m_robotDrive.SetExpiration(0.1);
+}
+
+void Robot::RobotInit() {
+ m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
+ m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
+ frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString line to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional comparisons to the
+ * if-else structure below with additional strings. If using the SendableChooser
+ * make sure to add them to the chooser code above as well.
+ */
+void Robot::Autonomous() {
+ std::string autoSelected = m_chooser.GetSelected();
+ // std::string autoSelected = frc::SmartDashboard::GetString(
+ // "Auto Selector", kAutoNameDefault);
+ std::cout << "Auto selected: " << autoSelected << std::endl;
+
+ // MotorSafety improves safety when motors are updated in loops but is
+ // disabled here because motor updates are not looped in this autonomous mode.
+ m_robotDrive.SetSafetyEnabled(false);
+
+ if (autoSelected == kAutoNameCustom) {
+ // Custom Auto goes here
+ std::cout << "Running custom Autonomous" << std::endl;
+
+ // Spin at half speed for two seconds
+ m_robotDrive.ArcadeDrive(0.0, 0.5);
+ frc::Wait(2.0);
+
+ // Stop robot
+ m_robotDrive.ArcadeDrive(0.0, 0.0);
+ } else {
+ // Default Auto goes here
+ std::cout << "Running default Autonomous" << std::endl;
+
+ // Drive forwards at half speed for two seconds
+ m_robotDrive.ArcadeDrive(-0.5, 0.0);
+ frc::Wait(2.0);
+
+ // Stop robot
+ m_robotDrive.ArcadeDrive(0.0, 0.0);
+ }
+}
+
+/**
+ * Runs the motors with arcade steering.
+ */
+void Robot::OperatorControl() {
+ m_robotDrive.SetSafetyEnabled(true);
+ while (IsOperatorControl() && IsEnabled()) {
+ // Drive with arcade style (use right stick)
+ m_robotDrive.ArcadeDrive(-m_stick.GetY(), m_stick.GetX());
+
+ // The motors will be updated every 5ms
+ frc::Wait(0.005);
+ }
+}
+
+/**
+ * Runs during test mode
+ */
+void Robot::Test() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/sample/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/sample/include/Robot.h
new file mode 100644
index 0000000..d568111
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/sample/include/Robot.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <string>
+
+#include <frc/Joystick.h>
+#include <frc/PWMVictorSPX.h>
+#include <frc/SampleRobot.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class. The
+ * SampleRobot class is the base of a robot application that will automatically
+ * call your Autonomous and OperatorControl methods at the right time as
+ * controlled by the switches on the driver station or the field controls.
+ *
+ * WARNING: While it may look like a good choice to use for your code if you're
+ * inexperienced, don't. Unless you know what you are doing, complex code will
+ * be much more difficult under this system. Use TimedRobot or Command-Based
+ * instead if you're new.
+ */
+class Robot : public frc::SampleRobot {
+ public:
+ Robot();
+
+ void RobotInit() override;
+ void Autonomous() override;
+ void OperatorControl() override;
+ void Test() override;
+
+ private:
+ // Robot drive system
+ frc::PWMVictorSPX m_leftMotor{0};
+ frc::PWMVictorSPX m_rightMotor{1};
+ frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+
+ frc::Joystick m_stick{0};
+
+ frc::SendableChooser<std::string> m_chooser;
+ const std::string kAutoNameDefault = "Default";
+ const std::string kAutoNameCustom = "My Auto";
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/templates.json b/wpilibcExamples/src/main/cpp/templates/templates.json
new file mode 100644
index 0000000..089ea16
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/templates.json
@@ -0,0 +1,47 @@
+[
+ {
+ "name": "Iterative Robot",
+ "description": "Iterative style",
+ "tags": [
+ "Iterative"
+ ],
+ "foldername": "iterative",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Timed Robot",
+ "description": "Timed style",
+ "tags": [
+ "Timed"
+ ],
+ "foldername": "timed",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Timed Skeleton (Advanced)",
+ "description": "Skeleton (stub) code for TimedRobot",
+ "tags": [
+ "Timed", "Skeleton"
+ ],
+ "foldername": "timedskeleton",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Command Robot",
+ "description": "Command style",
+ "tags": [
+ "Command"
+ ],
+ "foldername": "commandbased",
+ "gradlebase": "cpp"
+ },
+ {
+ "name": "Sample Robot",
+ "description": "Sample style",
+ "tags": [
+ "Sample"
+ ],
+ "foldername": "sample",
+ "gradlebase": "cpp"
+ }
+]
diff --git a/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp
new file mode 100644
index 0000000..07d843d
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <iostream>
+
+#include <frc/smartdashboard/SmartDashboard.h>
+
+void Robot::RobotInit() {
+ m_chooser.SetDefaultOption(kAutoNameDefault, kAutoNameDefault);
+ m_chooser.AddOption(kAutoNameCustom, kAutoNameCustom);
+ frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want ran during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {}
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString line to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional comparisons to the
+ * if-else structure below with additional strings. If using the SendableChooser
+ * make sure to add them to the chooser code above as well.
+ */
+void Robot::AutonomousInit() {
+ m_autoSelected = m_chooser.GetSelected();
+ // m_autoSelected = SmartDashboard::GetString("Auto Selector",
+ // kAutoNameDefault);
+ std::cout << "Auto selected: " << m_autoSelected << std::endl;
+
+ if (m_autoSelected == kAutoNameCustom) {
+ // Custom Auto goes here
+ } else {
+ // Default Auto goes here
+ }
+}
+
+void Robot::AutonomousPeriodic() {
+ if (m_autoSelected == kAutoNameCustom) {
+ // Custom Auto goes here
+ } else {
+ // Default Auto goes here
+ }
+}
+
+void Robot::TeleopInit() {}
+
+void Robot::TeleopPeriodic() {}
+
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h
new file mode 100644
index 0000000..09183dc
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-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 <string>
+
+#include <frc/TimedRobot.h>
+#include <frc/smartdashboard/SendableChooser.h>
+
+class Robot : public frc::TimedRobot {
+ public:
+ void RobotInit() override;
+ void RobotPeriodic() override;
+ void AutonomousInit() override;
+ void AutonomousPeriodic() override;
+ void TeleopInit() override;
+ void TeleopPeriodic() override;
+ void TestPeriodic() override;
+
+ private:
+ frc::SendableChooser<std::string> m_chooser;
+ const std::string kAutoNameDefault = "Default";
+ const std::string kAutoNameCustom = "My Auto";
+ std::string m_autoSelected;
+};
diff --git a/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp
new file mode 100644
index 0000000..76adfc4
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* 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"
+
+void Robot::RobotInit() {}
+
+void Robot::AutonomousInit() {}
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {}
+void Robot::TeleopPeriodic() {}
+
+void Robot::TestInit() {}
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() { return frc::StartRobot<Robot>(); }
+#endif
diff --git a/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h b/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h
new file mode 100644
index 0000000..bf4dae1
--- /dev/null
+++ b/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/TimedRobot.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;
+};