Put in new allwiplib-2018 and packaged the large files
added new allwpilib
added ntcore
Added new wpiutil
Change-Id: I5bbb966a69ac2fbdce056e4c092a13f246dbaa6a
diff --git a/third_party/allwpilib_2018/wpilibjExamples/build.gradle b/third_party/allwpilib_2018/wpilibjExamples/build.gradle
new file mode 100644
index 0000000..889d4a0
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/build.gradle
@@ -0,0 +1,30 @@
+apply plugin: 'java'
+apply plugin: 'pmd'
+
+dependencies {
+ compile project(':wpilibj')
+
+ compile 'edu.wpi.first.wpiutil:wpiutil-java:3.+'
+ compile 'edu.wpi.first.ntcore:ntcore-java:4.+'
+ compile 'org.opencv:opencv-java:3.2.0'
+ compile 'edu.wpi.first.cscore:cscore-java:1.+'
+}
+
+checkstyle {
+ configFile = new File(rootDir, "styleguide/checkstyleEclipse.xml")
+}
+
+pmd {
+ consoleOutput = true
+ reportsDir = file("$project.buildDir/reports/pmd")
+ ruleSetFiles = files(new File(rootDir, "styleguide/pmd-ruleset.xml"))
+ ruleSets = []
+}
+
+gradle.projectsEvaluated {
+ tasks.withType(JavaCompile) {
+ options.compilerArgs << "-Xlint:unchecked" << "-Xlint:deprecation"
+ }
+}
+
+apply from: 'publish.gradle'
diff --git a/third_party/allwpilib_2018/wpilibjExamples/examples.xml b/third_party/allwpilib_2018/wpilibjExamples/examples.xml
new file mode 100755
index 0000000..d8988ad
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/examples.xml
@@ -0,0 +1,340 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<examples>
+ <!-- Tags -->
+ <!-- Getting Started should be first and then alphabetical. Complete Example should be last -->
+ <tagDescription>
+ <name>Getting Started with Java</name>
+ <description>Examples for getting started with FRC Java</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Actuators</name>
+ <description>Example programs that demonstrate the use of various actuators</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Analog</name>
+ <description>Examples programs that show different uses of analog inputs,
+ outputs and various analog sensors</description>
+ </tagDescription>
+ <tagDescription>
+ <name>CAN</name>
+ <description>Example programs that demonstrate the use of the CAN components in the control
+ system</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Digital</name>
+ <description>Example programs that demonstrate the sensors that use the digital I/O ports</description>
+ </tagDescription>
+ <tagDescription>
+ <name>I2C</name>
+ <description>Example programs that demonstrate the use of I2C and various sensors that use
+ it</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Joystick</name>
+ <description>Example programs that demonstate different uses of joysticks for robot
+ driving</description>
+ </tagDescription>
+ <tagDescription>
+ <name>NetworkTables</name>
+ <description>Examples of how to use NetworkTables to accomplish a
+ variety of tasks such as sending and receiving values to both
+ dashboards and co-processors.</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Pneumatics</name>
+ <description>Example programs that demonstrate the use of the compressor and solenoids</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Robot and Motor</name>
+ <description>Example programs that demonstrate driving a robot and motors including safety,
+ servos, etc.</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Safety</name>
+ <description>Example programs that demonstate the motor safety classes and how to use them
+ with your programs</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Sensors</name>
+ <description>Example programs that demonstrate the use of the various commonly used sensors
+ on FRC robots</description>
+ </tagDescription>
+ <tagDescription>
+ <name>SPI</name>
+ <description>Example programs that demonstrate the use of the SPI bus and sensors that
+ connect to it</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Vision</name>
+ <description>Example programs that demonstrate the use of cameras and image processing</description>
+ </tagDescription>
+ <tagDescription>
+ <name>Complete Robot</name>
+ <description>Complete Robot example programs</description>
+ </tagDescription>
+
+ <!-- Examples -->
+ <example>
+ <name>Getting Started</name>
+ <description>An example program which demonstrates the simplest autonomous and
+ teleoperated routines.</description>
+ <tags>
+ <tag>Getting Started with Java</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/gettingstarted/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Tank Drive</name>
+ <description>Demonstrate the use of the RobotDrive class doing teleop driving with tank
+ steering</description>
+ <tags>
+ <tag>Actuators</tag>
+ <tag>Joystick</tag>
+ <tag>Robot and Motor</tag>
+ <tag>Safety</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/tankdrive/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Mecanum Drive</name>
+ <description>Demonstrate the use of the RobotDrive class doing teleop driving with Mecanum
+ steering</description>
+ <tags>
+ <tag>Actuators</tag>
+ <tag>Joystick</tag>
+ <tag>Robot and Motor</tag>
+ <tag>Safety</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/mecanumdrive/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Ultrasonic</name>
+ <description>Demonstrate maintaining a set distance using an ultrasonic sensor.</description>
+ <tags>
+ <tag>Sensors</tag>
+ <tag>Robot and Motor</tag>
+ <tag>Analog</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/ultrasonic/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Ultrasonic PID</name>
+ <description>Demonstrate maintaining a set distance using an ultrasonic sensor and PID
+ Control.</description>
+ <tags>
+ <tag>Sensors</tag>
+ <tag>Robot and Motor</tag>
+ <tag>Analog</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/ultrasonicpid/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Potentiometer PID</name>
+ <description>An example to demonstrate the use of a potentiometer and PID control to reach
+ elevator position setpoints.</description>
+ <tags>
+ <tag>Sensors</tag>
+ <tag>Actuators</tag>
+ <tag>Analog</tag>
+ <tag>Joystick</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/potentiometerpid/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Gyro</name>
+ <description>An example program showing how to drive straight with using a gyro sensor.</description>
+ <tags>
+ <tag>Sensors</tag>
+ <tag>Robot and Motor</tag>
+ <tag>Analog</tag>
+ <tag>Joystick</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/gyro/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Gyro Mecanum</name>
+ <description>An example program showing how to perform mecanum drive with field oriented
+ controls.</description>
+ <tags>
+ <tag>Sensors</tag>
+ <tag>Robot and Motor</tag>
+ <tag>Analog</tag>
+ <tag>Joystick</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/gyromecanum/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Motor Controller</name>
+ <description>Demonstrate controlling a single motor with a joystick</description>
+ <tags>
+ <tag>Actuators</tag>
+ <tag>Joystick</tag>
+ <tag>Robot and Motor</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/motorcontrol/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>GearsBot</name>
+ <description>A fully functional example CommandBased program for WPIs GearsBot robot. This
+ code can run on your computer if it supports simulation.</description>
+ <tags>
+ <tag>Complete Robot</tag>
+ </tags>
+ <world>/usr/share/frcsim/worlds/GearsBotDemo.world</world>
+ <packages>
+ <package>src/$package-dir</package>
+ <package>src/$package-dir/commands</package>
+ <package>src/$package-dir/subsystems</package>
+ </packages>
+ <files>
+ <file source="examples/gearsbot/OI.java" destination="src/$package-dir/OI.java" />
+ <file source="examples/gearsbot/Robot.java" destination="src/$package-dir/Robot.java" />
+ <file source="examples/gearsbot/commands/Autonomous.java" destination="src/$package-dir/commands/Autonomous.java" />
+ <file source="examples/gearsbot/commands/CloseClaw.java" destination="src/$package-dir/commands/CloseClaw.java" />
+ <file source="examples/gearsbot/commands/DriveStraight.java" destination="src/$package-dir/commands/DriveStraight.java" />
+ <file source="examples/gearsbot/commands/OpenClaw.java" destination="src/$package-dir/commands/OpenClaw.java" />
+ <file source="examples/gearsbot/commands/Pickup.java" destination="src/$package-dir/commands/Pickup.java" />
+ <file source="examples/gearsbot/commands/Place.java" destination="src/$package-dir/commands/Place.java" />
+ <file source="examples/gearsbot/commands/PrepareToPickup.java" destination="src/$package-dir/commands/PrepareToPickup.java" />
+ <file source="examples/gearsbot/commands/SetDistanceToBox.java" destination="src/$package-dir/commands/SetDistanceToBox.java" />
+ <file source="examples/gearsbot/commands/SetElevatorSetpoint.java" destination="src/$package-dir/commands/SetElevatorSetpoint.java" />
+ <file source="examples/gearsbot/commands/SetWristSetpoint.java" destination="src/$package-dir/commands/SetWristSetpoint.java" />
+ <file source="examples/gearsbot/commands/TankDriveWithJoystick.java" destination="src/$package-dir/commands/TankDriveWithJoystick.java" />
+ <file source="examples/gearsbot/subsystems/Claw.java" destination="src/$package-dir/subsystems/Claw.java" />
+ <file source="examples/gearsbot/subsystems/DriveTrain.java" destination="src/$package-dir/subsystems/DriveTrain.java" />
+ <file source="examples/gearsbot/subsystems/Elevator.java" destination="src/$package-dir/subsystems/Elevator.java" />
+ <file source="examples/gearsbot/subsystems/Wrist.java" destination="src/$package-dir/subsystems/Wrist.java" />
+ </files>
+ </example>
+ <example>
+ <name>PacGoat</name>
+ <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.</description>
+ <tags>
+ <tag>Complete Robot</tag>
+ </tags>
+ <world>/usr/share/frcsim/worlds/PacGoat2014.world</world>
+ <packages>
+ <package>src/$package-dir</package>
+ <package>src/$package-dir/commands</package>
+ <package>src/$package-dir/subsystems</package>
+ <package>src/$package-dir/triggers</package>
+ </packages>
+ <files>
+ <file source="examples/pacgoat/OI.java" destination="src/$package-dir/OI.java" />
+ <file source="examples/pacgoat/Robot.java" destination="src/$package-dir/Robot.java" />
+ <file source="examples/pacgoat/commands/CheckForHotGoal.java" destination="src/$package-dir/commands/CheckForHotGoal.java" />
+ <file source="examples/pacgoat/commands/CloseClaw.java" destination="src/$package-dir/commands/CloseClaw.java" />
+ <file source="examples/pacgoat/commands/Collect.java" destination="src/$package-dir/commands/Collect.java" />
+ <file source="examples/pacgoat/commands/DriveAndShootAutonomous.java" destination="src/$package-dir/commands/DriveAndShootAutonomous.java" />
+ <file source="examples/pacgoat/commands/DriveForward.java" destination="src/$package-dir/commands/DriveForward.java" />
+ <file source="examples/pacgoat/commands/DriveWithJoystick.java" destination="src/$package-dir/commands/DriveWithJoystick.java" />
+ <file source="examples/pacgoat/commands/ExtendShooter.java" destination="src/$package-dir/commands/ExtendShooter.java" />
+ <file source="examples/pacgoat/commands/LowGoal.java" destination="src/$package-dir/commands/LowGoal.java" />
+ <file source="examples/pacgoat/commands/OpenClaw.java" destination="src/$package-dir/commands/OpenClaw.java" />
+ <file source="examples/pacgoat/commands/SetCollectionSpeed.java" destination="src/$package-dir/commands/SetCollectionSpeed.java" />
+ <file source="examples/pacgoat/commands/SetPivotSetpoint.java" destination="src/$package-dir/commands/SetPivotSetpoint.java" />
+ <file source="examples/pacgoat/commands/Shoot.java" destination="src/$package-dir/commands/Shoot.java" />
+ <file source="examples/pacgoat/commands/WaitForBall.java" destination="src/$package-dir/commands/WaitForBall.java" />
+ <file source="examples/pacgoat/commands/WaitForPressure.java" destination="src/$package-dir/commands/WaitForPressure.java" />
+ <file source="examples/pacgoat/subsystems/Collector.java" destination="src/$package-dir/subsystems/Collector.java" />
+ <file source="examples/pacgoat/subsystems/DriveTrain.java" destination="src/$package-dir/subsystems/DriveTrain.java" />
+ <file source="examples/pacgoat/subsystems/Pivot.java" destination="src/$package-dir/subsystems/Pivot.java" />
+ <file source="examples/pacgoat/subsystems/Pneumatics.java" destination="src/$package-dir/subsystems/Pneumatics.java" />
+ <file source="examples/pacgoat/subsystems/Shooter.java" destination="src/$package-dir/subsystems/Shooter.java" />
+ <file source="examples/pacgoat/triggers/DoubleButton.java" destination="src/$package-dir/triggers/DoubleButton.java" />
+ </files>
+ </example>
+ <example>
+ <name>Simple Vision</name>
+ <description>Demonstrate the use of the CameraServer class to stream from a USB Webcam
+ without processing the images.</description>
+ <tags>
+ <tag>Vision</tag>
+ <tag>Complete List</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/quickvision/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Intermediate Vision</name>
+ <description>Demonstrate the use of the NIVision class to capture image from a Webcam,
+ process them, and then send them to the dashboard.</description>
+ <tags>
+ <tag>Vision</tag>
+ <tag>Complete List</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/intermediatevision/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+ <example>
+ <name>Axis Camera Sample</name>
+ <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.</description>
+ <tags>
+ <tag>Vision</tag>
+ </tags>
+ <packages>
+ <package>src/$package-dir</package>
+ </packages>
+ <files>
+ <file source="examples/axiscamera/Robot.java" destination="src/$package-dir/Robot.java" />
+ </files>
+ </example>
+</examples>
diff --git a/third_party/allwpilib_2018/wpilibjExamples/publish.gradle b/third_party/allwpilib_2018/wpilibjExamples/publish.gradle
new file mode 100644
index 0000000..81863c2
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/publish.gradle
@@ -0,0 +1,74 @@
+apply plugin: 'maven-publish'
+apply plugin: 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin'
+
+if (!hasProperty('releaseType')) {
+ WPILibVersion {
+ releaseType = 'dev'
+ }
+}
+
+def pubVersion
+if (project.hasProperty("publishVersion")) {
+ pubVersion = project.publishVersion
+} else {
+ pubVersion = WPILibVersion.version
+}
+
+def baseExamplesArtifactId = 'examples'
+def baseTemplatesArtifactId = 'templates'
+def artifactGroupId = 'edu.wpi.first.wpilibj'
+
+def outputsFolder = file("$project.buildDir/outputs")
+
+task javaExamplesZip(type: Zip) {
+ destinationDir = outputsFolder
+ baseName = 'wpilibj-examples'
+
+ from(licenseFile) {
+ into '/'
+ }
+
+ from('src/main/java/edu/wpi/first/wpilibj/examples') {
+ into 'examples'
+ }
+
+ from ('examples.xml') {
+ into 'examples'
+ }
+}
+
+task javaTemplatesZip(type: Zip) {
+ destinationDir = outputsFolder
+ baseName = 'wpilibj-templates'
+
+ from(licenseFile) {
+ into '/'
+ }
+
+ from('src/main/java/edu/wpi/first/wpilibj/templates') {
+ into 'templates'
+ }
+}
+
+build.dependsOn javaTemplatesZip
+build.dependsOn javaExamplesZip
+
+publishing {
+ publications {
+ examples(MavenPublication) {
+ artifact javaExamplesZip
+
+ artifactId = baseExamplesArtifactId
+ groupId artifactGroupId
+ version pubVersion
+ }
+
+ templates(MavenPublication) {
+ artifact javaTemplatesZip
+
+ artifactId = baseTemplatesArtifactId
+ groupId artifactGroupId
+ version pubVersion
+ }
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java
new file mode 100644
index 0000000..952823d
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.axiscamera;
+
+import edu.wpi.cscore.AxisCamera;
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.CvSource;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ * 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.
+ */
+public class Robot extends IterativeRobot {
+ Thread m_visionThread;
+
+ @Override
+ public void robotInit() {
+ m_visionThread = new Thread(() -> {
+ // Get the Axis camera from CameraServer
+ AxisCamera camera
+ = CameraServer.getInstance().addAxisCamera("axis-camera.local");
+ // Set the resolution
+ camera.setResolution(640, 480);
+
+ // Get a CvSink. This will capture Mats from the camera
+ CvSink cvSink = CameraServer.getInstance().getVideo();
+ // Setup a CvSource. This will send images back to the Dashboard
+ CvSource outputStream
+ = CameraServer.getInstance().putVideo("Rectangle", 640, 480);
+
+ // Mats are very memory expensive. Lets reuse this Mat.
+ Mat mat = new Mat();
+
+ // This cannot be 'true'. The program will never exit if it is. This
+ // lets the robot stop this thread when restarting robot code or
+ // deploying.
+ while (!Thread.interrupted()) {
+ // 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
+ Imgproc.rectangle(mat, new Point(100, 100), new Point(400, 400),
+ new Scalar(255, 255, 255), 5);
+ // Give the output stream a new image to display
+ outputStream.putFrame(mat);
+ }
+ });
+ m_visionThread.setDaemon(true);
+ m_visionThread.start();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java
new file mode 100644
index 0000000..1f664fe
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/OI.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.Pickup;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.Place;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.PrepareToPickup;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetElevatorSetpoint;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * This class is the glue that binds the controls on the physical operator
+ * interface to the commands and command groups that allow control of the robot.
+ */
+public class OI {
+ private Joystick m_joystick = new Joystick(0);
+
+ public OI() {
+ // Put Some buttons on the SmartDashboard
+ SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0));
+ SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2));
+ SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3));
+
+ SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0));
+ SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45));
+
+ SmartDashboard.putData("Open Claw", new OpenClaw());
+ SmartDashboard.putData("Close Claw", new CloseClaw());
+
+ SmartDashboard.putData("Deliver Soda", new Autonomous());
+
+ // Create some buttons
+ JoystickButton dpadUp = new JoystickButton(m_joystick, 5);
+ JoystickButton dpadRight = new JoystickButton(m_joystick, 6);
+ JoystickButton dpadDown = new JoystickButton(m_joystick, 7);
+ JoystickButton dpadLeft = new JoystickButton(m_joystick, 8);
+ JoystickButton l2 = new JoystickButton(m_joystick, 9);
+ JoystickButton r2 = new JoystickButton(m_joystick, 10);
+ JoystickButton l1 = new JoystickButton(m_joystick, 11);
+ JoystickButton r1 = new JoystickButton(m_joystick, 12);
+
+ // Connect the buttons to commands
+ dpadUp.whenPressed(new SetElevatorSetpoint(0.2));
+ dpadDown.whenPressed(new SetElevatorSetpoint(-0.2));
+ dpadRight.whenPressed(new CloseClaw());
+ dpadLeft.whenPressed(new OpenClaw());
+
+ r1.whenPressed(new PrepareToPickup());
+ r2.whenPressed(new Pickup());
+ l1.whenPressed(new Place());
+ l2.whenPressed(new Autonomous());
+ }
+
+ public Joystick getJoystick() {
+ return m_joystick;
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
new file mode 100644
index 0000000..7ecc808
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the manifest file in the resource
+ * directory.
+ */
+public class Robot extends IterativeRobot {
+ Command m_autonomousCommand;
+
+ public static DriveTrain m_drivetrain;
+ public static Elevator m_elevator;
+ public static Wrist m_wrist;
+ public static Claw m_claw;
+ public static OI m_oi;
+
+ /**
+ * This function is run when the robot is first started up and should be
+ * used for any initialization code.
+ */
+ @Override
+ public void robotInit() {
+ // Initialize all subsystems
+ m_drivetrain = new DriveTrain();
+ m_elevator = new Elevator();
+ m_wrist = new Wrist();
+ m_claw = new Claw();
+ m_oi = new OI();
+
+ // instantiate the command used for the autonomous period
+ m_autonomousCommand = new Autonomous();
+
+ // Show what command your subsystem is running on the SmartDashboard
+ SmartDashboard.putData(m_drivetrain);
+ SmartDashboard.putData(m_elevator);
+ SmartDashboard.putData(m_wrist);
+ SmartDashboard.putData(m_claw);
+ }
+
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand.start(); // schedule the autonomous command (example)
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+ log();
+ }
+
+ @Override
+ public void 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();
+ }
+
+ /**
+ * This function is called periodically during teleoperated mode.
+ */
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+ log();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+
+ /**
+ * The log method puts interesting information to the SmartDashboard.
+ */
+ private void log() {
+ m_wrist.log();
+ m_elevator.log();
+ m_drivetrain.log();
+ m_claw.log();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
new file mode 100644
index 0000000..5c3663c
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * The main autonomous command to pickup and deliver the soda to the box.
+ */
+public class Autonomous extends CommandGroup {
+ public 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/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
new file mode 100644
index 0000000..7b37c9e
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * Closes the claw for one second. Real robots should use sensors, stalling
+ * motors is BAD!
+ */
+public class CloseClaw extends Command {
+ public CloseClaw() {
+ requires(Robot.m_claw);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.m_claw.close();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.m_claw.isGrabbing();
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void 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.
+ if (!Robot.isSimulation()) {
+ Robot.m_claw.stop();
+ }
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
new file mode 100644
index 0000000..5f16990
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * 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.
+ */
+public class DriveStraight extends Command {
+ private PIDController m_pid;
+
+ public DriveStraight(double distance) {
+ requires(Robot.m_drivetrain);
+ m_pid = new PIDController(4, 0, 0, new PIDSource() {
+ PIDSourceType m_sourceType = PIDSourceType.kDisplacement;
+
+ @Override
+ public double pidGet() {
+ return Robot.m_drivetrain.getDistance();
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_sourceType = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_sourceType;
+ }
+ }, d -> Robot.m_drivetrain.drive(d, d));
+
+ m_pid.setAbsoluteTolerance(0.01);
+ m_pid.setSetpoint(distance);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ // Get everything in a safe starting state.
+ Robot.m_drivetrain.reset();
+ m_pid.reset();
+ m_pid.enable();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return m_pid.onTarget();
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ // Stop PID and the wheels
+ m_pid.disable();
+ Robot.m_drivetrain.drive(0, 0);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
new file mode 100644
index 0000000..e2a6963
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * Opens the claw for one second. Real robots should use sensors, stalling
+ * motors is BAD!
+ */
+public class OpenClaw extends TimedCommand {
+ public OpenClaw() {
+ super(1);
+ requires(Robot.m_claw);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.m_claw.open();
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.m_claw.stop();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
new file mode 100644
index 0000000..e017b20
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * Pickup a soda can (if one is between the open claws) and get it in a safe
+ * state to drive around.
+ */
+public class Pickup extends CommandGroup {
+ public Pickup() {
+ addSequential(new CloseClaw());
+ addParallel(new SetWristSetpoint(-45));
+ addSequential(new SetElevatorSetpoint(0.25));
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
new file mode 100644
index 0000000..0b4780c
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * Place a held soda can onto the platform.
+ */
+public class Place extends CommandGroup {
+ public Place() {
+ addSequential(new SetElevatorSetpoint(0.25));
+ addSequential(new SetWristSetpoint(0));
+ addSequential(new OpenClaw());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
new file mode 100644
index 0000000..94a8742
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+/**
+ * Make sure the robot is in a state to pickup soda cans.
+ */
+public class PrepareToPickup extends CommandGroup {
+ public PrepareToPickup() {
+ addParallel(new OpenClaw());
+ addParallel(new SetWristSetpoint(0));
+ addSequential(new SetElevatorSetpoint(0));
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
new file mode 100644
index 0000000..944ea56
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * 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.
+ */
+public class SetDistanceToBox extends Command {
+ private PIDController m_pid;
+
+ public SetDistanceToBox(double distance) {
+ requires(Robot.m_drivetrain);
+ m_pid = new PIDController(-2, 0, 0, new PIDSource() {
+ PIDSourceType m_sourceType = PIDSourceType.kDisplacement;
+
+ @Override
+ public double pidGet() {
+ return Robot.m_drivetrain.getDistanceToObstacle();
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ m_sourceType = pidSource;
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return m_sourceType;
+ }
+ }, d -> Robot.m_drivetrain.drive(d, d));
+
+ m_pid.setAbsoluteTolerance(0.01);
+ m_pid.setSetpoint(distance);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ // Get everything in a safe starting state.
+ Robot.m_drivetrain.reset();
+ m_pid.reset();
+ m_pid.enable();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return m_pid.onTarget();
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ // Stop PID and the wheels
+ m_pid.disable();
+ Robot.m_drivetrain.drive(0, 0);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
new file mode 100644
index 0000000..7de4ab1
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * 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!
+ */
+public class SetElevatorSetpoint extends Command {
+ private double m_setpoint;
+
+ public SetElevatorSetpoint(double setpoint) {
+ m_setpoint = setpoint;
+ requires(Robot.m_elevator);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.m_elevator.enable();
+ Robot.m_elevator.setSetpoint(m_setpoint);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.m_elevator.onTarget();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
new file mode 100644
index 0000000..7c8a744
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * 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!
+ */
+public class SetWristSetpoint extends Command {
+ private double m_setpoint;
+
+ public SetWristSetpoint(double setpoint) {
+ m_setpoint = setpoint;
+ requires(Robot.m_wrist);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.m_wrist.enable();
+ Robot.m_wrist.setSetpoint(m_setpoint);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.m_wrist.onTarget();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java
new file mode 100644
index 0000000..05d3bd5
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDriveWithJoystick.java
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+
+/**
+ * Have the robot drive tank style using the PS3 Joystick until interrupted.
+ */
+public class TankDriveWithJoystick extends Command {
+ public TankDriveWithJoystick() {
+ requires(Robot.m_drivetrain);
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ Robot.m_drivetrain.drive(Robot.m_oi.getJoystick());
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return false; // Runs until interrupted
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.m_drivetrain.drive(0, 0);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
new file mode 100644
index 0000000..1618016
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * 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.
+ */
+public class Claw extends Subsystem {
+ private Victor m_motor = new Victor(7);
+ private DigitalInput m_contact = new DigitalInput(5);
+
+ public Claw() {
+ super();
+
+ // Let's name everything on the LiveWindow
+ addChild("Motor", m_motor);
+ addChild("Limit Switch", m_contact);
+ }
+
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ public void log() {
+ }
+
+ /**
+ * Set the claw motor to move in the open direction.
+ */
+ public void open() {
+ m_motor.set(-1);
+ }
+
+ /**
+ * Set the claw motor to move in the close direction.
+ */
+ public void close() {
+ m_motor.set(1);
+ }
+
+ /**
+ * Stops the claw motor from moving.
+ */
+ public void stop() {
+ m_motor.set(0);
+ }
+
+ /**
+ * Return true when the robot is grabbing an object hard enough to trigger
+ * the limit switch.
+ */
+ public boolean isGrabbing() {
+ return m_contact.get();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
new file mode 100644
index 0000000..022b940
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
@@ -0,0 +1,144 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDriveWithJoystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * 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.
+ */
+public class DriveTrain extends Subsystem {
+ private SpeedController m_leftMotor
+ = new SpeedControllerGroup(new Spark(0), new Spark(1));
+ private SpeedController m_rightMotor
+ = new SpeedControllerGroup(new Spark(2), new Spark(3));
+
+ private DifferentialDrive m_drive
+ = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+ private Encoder m_leftEncoder = new Encoder(1, 2);
+ private Encoder m_rightEncoder = new Encoder(3, 4);
+ private AnalogInput m_rangefinder = new AnalogInput(6);
+ private AnalogGyro m_gyro = new AnalogGyro(1);
+
+ public DriveTrain() {
+ super();
+
+ // 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.
+ if (Robot.isReal()) {
+ m_leftEncoder.setDistancePerPulse(0.042);
+ m_rightEncoder.setDistancePerPulse(0.042);
+ } else {
+ // Circumference in ft = 4in/12(in/ft)*PI
+ m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
+ m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
+ }
+
+ // Let's name the sensors on the LiveWindow
+ addChild("Drive", m_drive);
+ addChild("Left Encoder", m_leftEncoder);
+ addChild("Right Encoder", m_rightEncoder);
+ addChild("Rangefinder", m_rangefinder);
+ addChild("Gyro", m_gyro);
+ }
+
+ /**
+ * When no other command is running let the operator drive around using the
+ * PS3 joystick.
+ */
+ @Override
+ public void initDefaultCommand() {
+ setDefaultCommand(new TankDriveWithJoystick());
+ }
+
+ /**
+ * The log method puts interesting information to the SmartDashboard.
+ */
+ public void log() {
+ SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance());
+ SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance());
+ SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate());
+ SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate());
+ SmartDashboard.putNumber("Gyro", m_gyro.getAngle());
+ }
+
+ /**
+ * Tank style driving for the DriveTrain.
+ *
+ * @param left
+ * Speed in range [-1,1]
+ * @param right
+ * Speed in range [-1,1]
+ */
+ public void drive(double left, double right) {
+ m_drive.tankDrive(left, right);
+ }
+
+ /**
+ * Tank style driving for the DriveTrain.
+ *
+ * @param joy The ps3 style joystick to use to drive tank style.
+ */
+ public void drive(Joystick joy) {
+ drive(-joy.getY(), -joy.getThrottle());
+ }
+
+ /**
+ * Get the robot's heading.
+ *
+ * @return The robots heading in degrees.
+ */
+ public double getHeading() {
+ return m_gyro.getAngle();
+ }
+
+ /**
+ * Reset the robots sensors to the zero states.
+ */
+ public void reset() {
+ m_gyro.reset();
+ m_leftEncoder.reset();
+ m_rightEncoder.reset();
+ }
+
+ /**
+ * Get the average distance of the encoders since the last reset.
+ *
+ * @return The distance driven (average of left and right encoders).
+ */
+ public double getDistance() {
+ return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2;
+ }
+
+ /**
+ * Get the distance to the obstacle.
+ *
+ * @return The distance to the obstacle detected by the rangefinder.
+ */
+ public double getDistanceToObstacle() {
+ // Really meters in simulation since it's a rangefinder...
+ return m_rangefinder.getAverageVoltage();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
new file mode 100644
index 0000000..a262643
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * 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.
+ */
+public class Elevator extends PIDSubsystem {
+ private Victor m_motor;
+ private AnalogPotentiometer m_pot;
+
+ private static final double kP_real = 4;
+ private static final double kI_real = 0.07;
+ private static final double kP_simulation = 18;
+ private static final double kI_simulation = 0.2;
+
+ public Elevator() {
+ super(kP_real, kI_real, 0);
+ if (Robot.isSimulation()) { // Check for simulation and update PID values
+ getPIDController().setPID(kP_simulation, kI_simulation, 0, 0);
+ }
+ setAbsoluteTolerance(0.005);
+
+ m_motor = new Victor(5);
+
+ // Conversion value of potentiometer varies between the real world and
+ // simulation
+ if (Robot.isReal()) {
+ m_pot = new AnalogPotentiometer(2, -2.0 / 5);
+ } else {
+ m_pot = new AnalogPotentiometer(2); // Defaults to meters
+ }
+
+ // Let's name everything on the LiveWindow
+ addChild("Motor", m_motor);
+ addChild("Pot", m_pot);
+ }
+
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ /**
+ * The log method puts interesting information to the SmartDashboard.
+ */
+ public void log() {
+ SmartDashboard.putData("Elevator Pot", (AnalogPotentiometer) m_pot);
+ }
+
+ /**
+ * Use the potentiometer as the PID sensor. This method is automatically
+ * called by the subsystem.
+ */
+ @Override
+ protected double returnPIDInput() {
+ return m_pot.get();
+ }
+
+ /**
+ * Use the motor as the PID output. This method is automatically called by
+ * the subsystem.
+ */
+ @Override
+ protected void usePIDOutput(double power) {
+ m_motor.set(power);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
new file mode 100644
index 0000000..96cc5ed
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * The wrist subsystem is like the elevator, but with a rotational joint instead
+ * of a linear joint.
+ */
+public class Wrist extends PIDSubsystem {
+ private Victor m_motor;
+ private AnalogPotentiometer m_pot;
+
+ private static final double kP_real = 1;
+ private static final double kP_simulation = 0.05;
+
+ public Wrist() {
+ super(kP_real, 0, 0);
+ if (Robot.isSimulation()) { // Check for simulation and update PID values
+ getPIDController().setPID(kP_simulation, 0, 0, 0);
+ }
+ setAbsoluteTolerance(2.5);
+
+ m_motor = new Victor(6);
+
+ // Conversion value of potentiometer varies between the real world and
+ // simulation
+ if (Robot.isReal()) {
+ m_pot = new AnalogPotentiometer(3, -270.0 / 5);
+ } else {
+ m_pot = new AnalogPotentiometer(3); // Defaults to degrees
+ }
+
+ // Let's name everything on the LiveWindow
+ addChild("Motor", m_motor);
+ addChild("Pot", m_pot);
+ }
+
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ /**
+ * The log method puts interesting information to the SmartDashboard.
+ */
+ public void log() {
+ SmartDashboard.putData("Wrist Angle", (AnalogPotentiometer) m_pot);
+ }
+
+ /**
+ * Use the potentiometer as the PID sensor. This method is automatically
+ * called by the subsystem.
+ */
+ @Override
+ protected double returnPIDInput() {
+ return m_pot.get();
+ }
+
+ /**
+ * Use the motor as the PID output. This method is automatically called by
+ * the subsystem.
+ */
+ @Override
+ protected void usePIDOutput(double power) {
+ m_motor.set(power);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
new file mode 100644
index 0000000..3ddb6e5
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gettingstarted;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the manifest file in the resource
+ * directory.
+ */
+public class Robot extends IterativeRobot {
+ private DifferentialDrive m_robotDrive
+ = new DifferentialDrive(new Spark(0), new Spark(1));
+ private Joystick m_stick = new Joystick(0);
+ private Timer m_timer = new Timer();
+
+ /**
+ * This function is run when the robot is first started up and should be
+ * used for any initialization code.
+ */
+ @Override
+ public void robotInit() {
+ }
+
+ /**
+ * This function is run once each time the robot enters autonomous mode.
+ */
+ @Override
+ public void autonomousInit() {
+ m_timer.reset();
+ m_timer.start();
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ // Drive for 2 seconds
+ if (m_timer.get() < 2.0) {
+ m_robotDrive.arcadeDrive(0.5, 0.0); // drive forwards half speed
+ } else {
+ m_robotDrive.stopMotor(); // stop robot
+ }
+ }
+
+ /**
+ * This function is called once each time the robot enters teleoperated mode.
+ */
+ @Override
+ public void teleopInit() {
+ }
+
+ /**
+ * This function is called periodically during teleoperated mode.
+ */
+ @Override
+ public void teleopPeriodic() {
+ m_robotDrive.arcadeDrive(m_stick.getY(), m_stick.getX());
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
new file mode 100644
index 0000000..f22264b
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyro;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * 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.
+ */
+public class Robot extends IterativeRobot {
+ private static final double kAngleSetpoint = 0.0;
+ private static final double kP = 0.005; // propotional turning constant
+
+ // gyro calibration constant, may need to be adjusted;
+ // gyro value of 360 is set to correspond to one full revolution
+ private static final double kVoltsPerDegreePerSecond = 0.0128;
+
+ private static final int kLeftMotorPort = 0;
+ private static final int kRightMotorPort = 1;
+ private static final int kGyroPort = 0;
+ private static final int kJoystickPort = 0;
+
+ private DifferentialDrive m_myRobot
+ = new DifferentialDrive(new Spark(kLeftMotorPort),
+ new Spark(kRightMotorPort));
+ private AnalogGyro m_gyro = new AnalogGyro(kGyroPort);
+ private Joystick m_joystick = new Joystick(kJoystickPort);
+
+ @Override
+ public void robotInit() {
+ m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
+ }
+
+ /**
+ * The motor speed is set from the joystick while the RobotDrive turning
+ * value is assigned from the error between the setpoint and the gyro angle.
+ */
+ @Override
+ public void teleopPeriodic() {
+ double turningValue = (kAngleSetpoint - m_gyro.getAngle()) * kP;
+ // Invert the direction of the turn if we are going backwards
+ turningValue = Math.copySign(turningValue, m_joystick.getY());
+ m_myRobot.arcadeDrive(m_joystick.getY(), turningValue);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
new file mode 100644
index 0000000..7cc115f
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.gyromecanum;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.MecanumDrive;
+
+/**
+ * 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).
+ */
+public class Robot extends IterativeRobot {
+ // gyro calibration constant, may need to be adjusted;
+ // gyro value of 360 is set to correspond to one full revolution
+ private static final double kVoltsPerDegreePerSecond = 0.0128;
+
+ private static final int kFrontLeftChannel = 0;
+ private static final int kRearLeftChannel = 1;
+ private static final int kFrontRightChannel = 2;
+ private static final int kRearRightChannel = 3;
+ private static final int kGyroPort = 0;
+ private static final int kJoystickPort = 0;
+
+ private MecanumDrive m_robotDrive;
+ private AnalogGyro m_gyro = new AnalogGyro(kGyroPort);
+ private Joystick m_joystick = new Joystick(kJoystickPort);
+
+ @Override
+ public void robotInit() {
+ Spark frontLeft = new Spark(kFrontLeftChannel);
+ Spark rearLeft = new Spark(kRearLeftChannel);
+ Spark frontRight = new Spark(kFrontRightChannel);
+ Spark rearRight = new Spark(kRearRightChannel);
+
+ // Invert the left side motors.
+ // You may need to change or remove this to match your robot.
+ frontLeft.setInverted(true);
+ rearLeft.setInverted(true);
+
+ m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+ m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
+ }
+
+ /**
+ * Mecanum drive is used with the gyro angle as an input.
+ */
+ @Override
+ public void teleopPeriodic() {
+ m_robotDrive.driveCartesian(m_joystick.getX(), m_joystick.getY(),
+ m_joystick.getZ(), m_gyro.getAngle());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java
new file mode 100644
index 0000000..738a3ea
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java
@@ -0,0 +1,68 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.intermediatevision;
+
+import edu.wpi.cscore.CvSink;
+import edu.wpi.cscore.CvSource;
+import edu.wpi.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ * 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.
+ */
+public class Robot extends IterativeRobot {
+ Thread m_visionThread;
+
+ @Override
+ public void robotInit() {
+ m_visionThread = new Thread(() -> {
+ // Get the UsbCamera from CameraServer
+ UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
+ // Set the resolution
+ camera.setResolution(640, 480);
+
+ // Get a CvSink. This will capture Mats from the camera
+ CvSink cvSink = CameraServer.getInstance().getVideo();
+ // Setup a CvSource. This will send images back to the Dashboard
+ CvSource outputStream
+ = CameraServer.getInstance().putVideo("Rectangle", 640, 480);
+
+ // Mats are very memory expensive. Lets reuse this Mat.
+ Mat mat = new Mat();
+
+ // This cannot be 'true'. The program will never exit if it is. This
+ // lets the robot stop this thread when restarting robot code or
+ // deploying.
+ while (!Thread.interrupted()) {
+ // 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
+ Imgproc.rectangle(mat, new Point(100, 100), new Point(400, 400),
+ new Scalar(255, 255, 255), 5);
+ // Give the output stream a new image to display
+ outputStream.putFrame(mat);
+ }
+ });
+ m_visionThread.setDaemon(true);
+ m_visionThread.start();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
new file mode 100755
index 0000000..20076ae
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.mecanumdrive;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.MecanumDrive;
+
+/**
+ * This is a demo program showing how to use Mecanum control with the RobotDrive
+ * class.
+ */
+public class Robot extends IterativeRobot {
+ private static final int kFrontLeftChannel = 2;
+ private static final int kRearLeftChannel = 3;
+ private static final int kFrontRightChannel = 1;
+ private static final int kRearRightChannel = 0;
+
+ private static final int kJoystickChannel = 0;
+
+ private MecanumDrive m_robotDrive;
+ private Joystick m_stick;
+
+ @Override
+ public void robotInit() {
+ Spark frontLeft = new Spark(kFrontLeftChannel);
+ Spark rearLeft = new Spark(kRearLeftChannel);
+ Spark frontRight = new Spark(kFrontRightChannel);
+ Spark rearRight = new Spark(kRearRightChannel);
+
+ // Invert the left side motors.
+ // You may need to change or remove this to match your robot.
+ frontLeft.setInverted(true);
+ rearLeft.setInverted(true);
+
+ m_robotDrive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);
+
+ m_stick = new Joystick(kJoystickChannel);
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ // 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(), 0.0);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
new file mode 100755
index 0000000..9811eab
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.motorcontrol;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Spark;
+
+/**
+ * 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.
+ *
+ * <p>Joystick analog values range from -1 to 1 and speed controller inputs also
+ * range from -1 to 1 making it easy to work together.
+ */
+public class Robot extends IterativeRobot {
+ private static final int kMotorPort = 0;
+ private static final int kJoystickPort = 0;
+
+ private SpeedController m_motor;
+ private Joystick m_joystick;
+
+ @Override
+ public void robotInit() {
+ m_motor = new Spark(kMotorPort);
+ m_joystick = new Joystick(kJoystickPort);
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ m_motor.set(m_joystick.getY());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java
new file mode 100644
index 0000000..d66e012
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.Collect;
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveForward;
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.LowGoal;
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.SetCollectionSpeed;
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.SetPivotSetpoint;
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.Shoot;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
+import edu.wpi.first.wpilibj.examples.pacgoat.triggers.DoubleButton;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * The operator interface of the robot, it has been simplified from the real
+ * robot to allow control with a single PS3 joystick. As a result, not all
+ * functionality from the real robot is available.
+ */
+public class OI {
+ public Joystick m_joystick = new Joystick(0);
+
+ public OI() {
+ new JoystickButton(m_joystick, 12).whenPressed(new LowGoal());
+ new JoystickButton(m_joystick, 10).whenPressed(new Collect());
+
+ new JoystickButton(m_joystick, 11).whenPressed(
+ new SetPivotSetpoint(Pivot.kShoot));
+ new JoystickButton(m_joystick, 9).whenPressed(
+ new SetPivotSetpoint(Pivot.kShootNear));
+
+ new DoubleButton(m_joystick, 2, 3).whenActive(new Shoot());
+
+ // SmartDashboard Buttons
+ SmartDashboard.putData("Drive Forward", new DriveForward(2.25));
+ SmartDashboard.putData("Drive Backward", new DriveForward(-2.25));
+ SmartDashboard.putData("Start Rollers",
+ new SetCollectionSpeed(Collector.kForward));
+ SmartDashboard.putData("Stop Rollers",
+ new SetCollectionSpeed(Collector.kStop));
+ SmartDashboard.putData("Reverse Rollers",
+ new SetCollectionSpeed(Collector.kReverse));
+ }
+
+ public Joystick getJoystick() {
+ return m_joystick;
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
new file mode 100644
index 0000000..6de3eb3
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveAndShootAutonomous;
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveForward;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pneumatics;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Shooter;
+
+/**
+ * This is the main class for running the PacGoat code.
+ *
+ * <p>The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the manifest file in the resource
+ * directory.
+ */
+public class Robot extends IterativeRobot {
+ Command m_autonomousCommand;
+ public static OI oi;
+
+ // Initialize the subsystems
+ public static DriveTrain drivetrain = new DriveTrain();
+ public static Collector collector = new Collector();
+ public static Shooter shooter = new Shooter();
+ public static Pneumatics pneumatics = new Pneumatics();
+ public static Pivot pivot = new Pivot();
+
+ public SendableChooser<Command> m_autoChooser;
+
+ // This function is run when the robot is first started up and should be
+ // used for any initialization code.
+ @Override
+ public void robotInit() {
+ SmartDashboard.putData(drivetrain);
+ SmartDashboard.putData(collector);
+ SmartDashboard.putData(shooter);
+ SmartDashboard.putData(pneumatics);
+ SmartDashboard.putData(pivot);
+
+ // This MUST be here. If the OI creates Commands (which it very likely
+ // will), constructing it during the construction of CommandBase (from
+ // which commands extend), subsystems are not guaranteed to be
+ // yet. Thus, their requires() statements may grab null pointers. Bad
+ // news. Don't move it.
+ oi = new OI();
+
+ // instantiate the command used for the autonomous period
+ m_autoChooser = new SendableChooser<Command>();
+ m_autoChooser.addDefault("Drive and Shoot", new DriveAndShootAutonomous());
+ m_autoChooser.addObject("Drive Forward", new DriveForward());
+ SmartDashboard.putData("Auto Mode", m_autoChooser);
+ }
+
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = (Command) m_autoChooser.getSelected();
+ m_autonomousCommand.start();
+ }
+
+ // This function is called periodically during autonomous
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+ log();
+ }
+
+ @Override
+ public void 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 != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ // This function is called periodically during operator control
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+ log();
+ }
+
+ // This function called periodically during test mode
+ @Override
+ public void testPeriodic() {
+ }
+
+ @Override
+ public void disabledInit() {
+ Robot.shooter.unlatch();
+ }
+
+ // This function is called periodically while disabled
+ @Override
+ public void disabledPeriodic() {
+ log();
+ }
+
+ /**
+ * Log interesting values to the SmartDashboard.
+ */
+ private void log() {
+ Robot.pneumatics.writePressure();
+ SmartDashboard.putNumber("Pivot Pot Value", Robot.pivot.getAngle());
+ SmartDashboard.putNumber("Left Distance",
+ drivetrain.getLeftEncoder().getDistance());
+ SmartDashboard.putNumber("Right Distance",
+ drivetrain.getRightEncoder().getDistance());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java
new file mode 100644
index 0000000..16d4c8b
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * 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.
+ */
+public class CheckForHotGoal extends Command {
+ public CheckForHotGoal(double time) {
+ setTimeout(time);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return isTimedOut() || Robot.shooter.goalIsHot();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java
new file mode 100644
index 0000000..251e756
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Close the claw.
+ *
+ * <p>NOTE: It doesn't wait for the claw to close since there is no sensor to
+ * detect that.
+ */
+public class CloseClaw extends InstantCommand {
+ public CloseClaw() {
+ requires(Robot.collector);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.collector.close();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java
new file mode 100644
index 0000000..21d9f35
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
+
+/**
+ * Get the robot set to collect balls.
+ */
+public class Collect extends CommandGroup {
+ public Collect() {
+ addSequential(new SetCollectionSpeed(Collector.kForward));
+ addParallel(new CloseClaw());
+ addSequential(new SetPivotSetpoint(Pivot.kCollect));
+ addSequential(new WaitForBall());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java
new file mode 100644
index 0000000..fcce7ed
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Drive over the line and then shoot the ball. If the hot goal is not detected,
+ * it will wait briefly.
+ */
+public class DriveAndShootAutonomous extends CommandGroup {
+ public DriveAndShootAutonomous() {
+ addSequential(new CloseClaw());
+ addSequential(new WaitForPressure(), 2);
+ if (Robot.isReal()) {
+ // NOTE: Simulation doesn't currently have the concept of hot.
+ addSequential(new CheckForHotGoal(2));
+ }
+ addSequential(new SetPivotSetpoint(45));
+ addSequential(new DriveForward(8, 0.3));
+ addSequential(new Shoot());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java
new file mode 100644
index 0000000..8562607
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * 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.
+ */
+public class DriveForward extends Command {
+ private double m_driveForwardSpeed;
+ private double m_distance;
+ private double m_error;
+ private static final double kTolerance = 0.1;
+ private static final double kP = -1.0 / 5.0;
+
+ public DriveForward() {
+ this(10, 0.5);
+ }
+
+ public DriveForward(double dist) {
+ this(dist, 0.5);
+ }
+
+ public DriveForward(double dist, double maxSpeed) {
+ requires(Robot.drivetrain);
+ m_distance = dist;
+ m_driveForwardSpeed = maxSpeed;
+ }
+
+ @Override
+ protected void initialize() {
+ Robot.drivetrain.getRightEncoder().reset();
+ setTimeout(2);
+ }
+
+ @Override
+ protected void 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);
+ }
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return Math.abs(m_error) <= kTolerance || isTimedOut();
+ }
+
+ @Override
+ protected void end() {
+ Robot.drivetrain.stop();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java
new file mode 100644
index 0000000..580fc8a
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * This command allows PS3 joystick to drive the robot. It is always running
+ * except when interrupted by another command.
+ */
+public class DriveWithJoystick extends Command {
+ public DriveWithJoystick() {
+ requires(Robot.drivetrain);
+ }
+
+ @Override
+ protected void execute() {
+ Robot.drivetrain.tankDrive(Robot.oi.getJoystick());
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ @Override
+ protected void end() {
+ Robot.drivetrain.stop();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java
new file mode 100644
index 0000000..07857ce
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.TimedCommand;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Extend the shooter and then retract it after a second.
+ */
+public class ExtendShooter extends TimedCommand {
+ public ExtendShooter() {
+ super(1);
+ requires(Robot.shooter);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.shooter.extendBoth();
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ Robot.shooter.retractBoth();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java
new file mode 100644
index 0000000..bb5247c
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
+
+/**
+ * Spit the ball out into the low goal assuming that the robot is in front of
+ * it.
+ */
+public class LowGoal extends CommandGroup {
+ public LowGoal() {
+ addSequential(new SetPivotSetpoint(Pivot.kLowGoal));
+ addSequential(new SetCollectionSpeed(Collector.kReverse));
+ addSequential(new ExtendShooter());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java
new file mode 100644
index 0000000..ef9e72a
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * Opens the claw.
+ */
+public class OpenClaw extends Command {
+ public OpenClaw() {
+ requires(Robot.collector);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.collector.open();
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.collector.isOpen();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java
new file mode 100644
index 0000000..28a6986
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.InstantCommand;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * 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.
+ */
+public class SetCollectionSpeed extends InstantCommand {
+ private double m_speed;
+
+ public SetCollectionSpeed(double speed) {
+ requires(Robot.collector);
+ this.m_speed = speed;
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.collector.setSpeed(m_speed);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java
new file mode 100644
index 0000000..5873eb1
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * 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!
+ */
+public class SetPivotSetpoint extends Command {
+ private double m_setpoint;
+
+ public SetPivotSetpoint(double setpoint) {
+ this.m_setpoint = setpoint;
+ requires(Robot.pivot);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ Robot.pivot.enable();
+ Robot.pivot.setSetpoint(m_setpoint);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.pivot.onTarget();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java
new file mode 100644
index 0000000..ba7477f
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
+
+/**
+ * Shoot the ball at the current angle.
+ */
+public class Shoot extends CommandGroup {
+ public Shoot() {
+ addSequential(new WaitForPressure());
+ addSequential(new SetCollectionSpeed(Collector.kStop));
+ addSequential(new OpenClaw());
+ addSequential(new ExtendShooter());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java
new file mode 100644
index 0000000..4a27abb
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * 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.
+ */
+public class WaitForBall extends Command {
+ public WaitForBall() {
+ requires(Robot.collector);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.collector.hasBall();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java
new file mode 100644
index 0000000..14d03a4
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * 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.
+ */
+public class WaitForPressure extends Command {
+ public WaitForPressure() {
+ requires(Robot.pneumatics);
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return Robot.pneumatics.isPressurized();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
new file mode 100644
index 0000000..70b2827
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * 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.
+ */
+public class Collector extends Subsystem {
+ // Constants for some useful speeds
+ public static final double kForward = 1;
+ public static final double kStop = 0;
+ public static final double kReverse = -1;
+
+ // Subsystem devices
+ private SpeedController m_rollerMotor = new Victor(6);
+ private DigitalInput m_ballDetector = new DigitalInput(10);
+ private DigitalInput m_openDetector = new DigitalInput(6);
+ private Solenoid m_piston = new Solenoid(1, 1);
+
+ public Collector() {
+ // Put everything to the LiveWindow for testing.
+ addChild("Roller Motor", (Victor) m_rollerMotor);
+ addChild("Ball Detector", m_ballDetector);
+ addChild("Claw Open Detector", m_openDetector);
+ addChild("Piston", m_piston);
+ }
+
+ /**
+ * Whether or not the robot has the ball.
+ *
+ * <p>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.
+ */
+ public boolean hasBall() {
+ return m_ballDetector.get(); // TODO: prepend ! to reflect real robot
+ }
+
+ /**
+ * Set the speed to spin the collector rollers.
+ *
+ * @param speed The speed to spin the rollers.
+ */
+ public void setSpeed(double speed) {
+ m_rollerMotor.set(-speed);
+ }
+
+ /**
+ * Stop the rollers from spinning.
+ */
+ public void stop() {
+ m_rollerMotor.set(0);
+ }
+
+ /**
+ * Wether or not the claw is open.
+ *
+ * @return Whether or not the claw is open.
+ */
+ public boolean isOpen() {
+ return m_openDetector.get(); // TODO: prepend ! to reflect real robot
+ }
+
+ /**
+ * Open the claw up (For shooting).
+ */
+ public void open() {
+ m_piston.set(true);
+ }
+
+ /**
+ * Close the claw (For collecting and driving).
+ */
+ public void close() {
+ m_piston.set(false);
+ }
+
+ /**
+ * No default command.
+ */
+ @Override
+ protected void initDefaultCommand() {
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
new file mode 100644
index 0000000..9b34267
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
@@ -0,0 +1,140 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.CounterBase.EncodingType;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.SpeedControllerGroup;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveWithJoystick;
+
+/**
+ * The DriveTrain subsystem controls the robot's chassis and reads in
+ * information about it's speed and position.
+ */
+public class DriveTrain extends Subsystem {
+ // Subsystem devices
+ private SpeedController m_frontLeftCIM = new Victor(1);
+ private SpeedController m_frontRightCIM = new Victor(2);
+ private SpeedController m_rearLeftCIM = new Victor(3);
+ private SpeedController m_rearRightCIM = new Victor(4);
+ private SpeedControllerGroup m_leftCIMs = new SpeedControllerGroup(
+ m_frontLeftCIM, m_rearLeftCIM);
+ private SpeedControllerGroup m_rightCIMs = new SpeedControllerGroup(
+ m_frontRightCIM, m_rearRightCIM);
+ private DifferentialDrive m_drive;
+ private Encoder m_rightEncoder = new Encoder(1, 2, true, EncodingType.k4X);
+ private Encoder m_leftEncoder = new Encoder(3, 4, false, EncodingType.k4X);
+ private AnalogGyro m_gyro = new AnalogGyro(2);
+
+ public DriveTrain() {
+ // Configure drive motors
+ addChild("Front Left CIM", (Victor) m_frontLeftCIM);
+ addChild("Front Right CIM", (Victor) m_frontRightCIM);
+ addChild("Back Left CIM", (Victor) m_rearLeftCIM);
+ addChild("Back Right CIM", (Victor) m_rearRightCIM);
+
+ // Configure the DifferentialDrive to reflect the fact that all motors
+ // are wired backwards (right is inverted in DifferentialDrive).
+ m_leftCIMs.setInverted(true);
+ m_drive = new DifferentialDrive(m_leftCIMs, m_rightCIMs);
+ m_drive.setSafetyEnabled(true);
+ m_drive.setExpiration(0.1);
+ m_drive.setMaxOutput(1.0);
+
+ // Configure encoders
+ m_rightEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
+ m_leftEncoder.setPIDSourceType(PIDSourceType.kDisplacement);
+
+ if (Robot.isReal()) { // Converts to feet
+ m_rightEncoder.setDistancePerPulse(0.0785398);
+ m_leftEncoder.setDistancePerPulse(0.0785398);
+ } else {
+ // Convert to feet 4in diameter wheels with 360 tick sim encoders
+ m_rightEncoder.setDistancePerPulse(
+ (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */));
+ m_leftEncoder.setDistancePerPulse(
+ (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */));
+ }
+
+ addChild("Right Encoder", m_rightEncoder);
+ addChild("Left Encoder", m_leftEncoder);
+
+ // Configure gyro
+ if (Robot.isReal()) {
+ m_gyro.setSensitivity(0.007); // TODO: Handle more gracefully?
+ }
+ addChild("Gyro", m_gyro);
+ }
+
+ /**
+ * When other commands aren't using the drivetrain, allow tank drive with
+ * the joystick.
+ */
+ @Override
+ public void initDefaultCommand() {
+ setDefaultCommand(new DriveWithJoystick());
+ }
+
+ /**
+ * Tank drive using a PS3 joystick.
+ *
+ * @param joy PS3 style joystick to use as the input for tank drive.
+ */
+ public void tankDrive(Joystick joy) {
+ m_drive.tankDrive(joy.getY(), joy.getRawAxis(4));
+ }
+
+ /**
+ * Tank drive using individual joystick axes.
+ *
+ * @param leftAxis Left sides value
+ * @param rightAxis Right sides value
+ */
+ public void tankDrive(double leftAxis, double rightAxis) {
+ m_drive.tankDrive(leftAxis, rightAxis);
+ }
+
+ /**
+ * Stop the drivetrain from moving.
+ */
+ public void stop() {
+ m_drive.tankDrive(0, 0);
+ }
+
+ /**
+ * The encoder getting the distance and speed of left side of the
+ * drivetrain.
+ */
+ public Encoder getLeftEncoder() {
+ return m_leftEncoder;
+ }
+
+ /**
+ * The encoder getting the distance and speed of right side of the
+ * drivetrain.
+ */
+ public Encoder getRightEncoder() {
+ return m_rightEncoder;
+ }
+
+ /**
+ * The current angle of the drivetrain as measured by the Gyro.
+ */
+ public double getAngle() {
+ return m_gyro.getAngle();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java
new file mode 100644
index 0000000..be2199d
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.command.PIDSubsystem;
+import edu.wpi.first.wpilibj.interfaces.Potentiometer;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * The Pivot subsystem contains the Van-door motor and the pot for PID control
+ * of angle of the pivot and claw.
+ */
+public class Pivot extends PIDSubsystem {
+ // Constants for some useful angles
+ public static final double kCollect = 105;
+ public static final double kLowGoal = 90;
+ public static final double kShoot = 45;
+ public static final double kShootNear = 30;
+
+ // Sensors for measuring the position of the pivot.
+ private DigitalInput m_upperLimitSwitch = new DigitalInput(13);
+ private DigitalInput m_lowerLimitSwitch = new DigitalInput(12);
+
+ // 0 degrees is vertical facing up.
+ // Angle increases the more forward the pivot goes.
+ private Potentiometer m_pot = new AnalogPotentiometer(1);
+
+ // Motor to move the pivot.
+ private SpeedController m_motor = new Victor(5);
+
+ public Pivot() {
+ super("Pivot", 7.0, 0.0, 8.0);
+ setAbsoluteTolerance(0.005);
+ getPIDController().setContinuous(false);
+ if (Robot.isSimulation()) { // PID is different in simulation.
+ getPIDController().setPID(0.5, 0.001, 2);
+ setAbsoluteTolerance(5);
+ }
+
+ // Put everything to the LiveWindow for testing.
+ addChild("Upper Limit Switch", m_upperLimitSwitch);
+ addChild("Lower Limit Switch", m_lowerLimitSwitch);
+ addChild("Pot", (AnalogPotentiometer) m_pot);
+ addChild("Motor", (Victor) m_motor);
+ addChild("PIDSubsystem Controller", getPIDController());
+ }
+
+ /**
+ * No default command, if PID is enabled, the current setpoint will be
+ * maintained.
+ */
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ /**
+ * The angle read in by the potentiometer.
+ */
+ @Override
+ protected double returnPIDInput() {
+ return m_pot.get();
+ }
+
+ /**
+ * Set the motor speed based off of the PID output.
+ */
+ @Override
+ protected void usePIDOutput(double output) {
+ m_motor.pidWrite(output);
+ }
+
+ /**
+ * If the pivot is at its upper limit.
+ */
+ public boolean isAtUpperLimit() {
+ // TODO: inverted from real robot (prefix with !)
+ return m_upperLimitSwitch.get();
+ }
+
+ /**
+ * If the pivot is at its lower limit.
+ */
+ public boolean isAtLowerLimit() {
+ // TODO: inverted from real robot (prefix with !)
+ return m_lowerLimitSwitch.get();
+ }
+
+ /**
+ * The current angle of the pivot.
+ */
+ public double getAngle() {
+ return m_pot.get();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java
new file mode 100644
index 0000000..61fc155
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
+
+/**
+ * The Pneumatics subsystem contains a pressure sensor.
+ *
+ * <p>NOTE: The simulator currently doesn't support the compressor or pressure
+ * sensors.
+ */
+public class Pneumatics extends Subsystem {
+ AnalogInput m_pressureSensor = new AnalogInput(3);
+
+ private static final double kMaxPressure = 2.55;
+
+ public Pneumatics() {
+ addChild("Pressure Sensor", m_pressureSensor);
+ }
+
+ /**
+ * No default command.
+ */
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ /**
+ * Whether or not the system is fully pressurized.
+ */
+ public boolean isPressurized() {
+ if (Robot.isReal()) {
+ return kMaxPressure <= m_pressureSensor.getVoltage();
+ } else {
+ return true; // NOTE: Simulation always has full pressure
+ }
+ }
+
+ /**
+ * Puts the pressure on the SmartDashboard.
+ */
+ public void writePressure() {
+ SmartDashboard.putNumber("Pressure", m_pressureSensor.getVoltage());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
new file mode 100644
index 0000000..57b5ab2
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
@@ -0,0 +1,173 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * 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.
+ *
+ * <p>NOTE: Simulation currently approximates this as as single pneumatic
+ * cylinder and ignores the latch.
+ */
+public class Shooter extends Subsystem {
+ // Devices
+ DoubleSolenoid m_piston1 = new DoubleSolenoid(1, 3, 4);
+ DoubleSolenoid m_piston2 = new DoubleSolenoid(1, 5, 6);
+ Solenoid m_latchPiston = new Solenoid(1, 2);
+ DigitalInput m_piston1ReedSwitchFront = new DigitalInput(9);
+ DigitalInput m_piston1ReedSwitchBack = new DigitalInput(11);
+ //NOTE: currently ignored in simulation
+ DigitalInput m_hotGoalSensor = new DigitalInput(3);
+
+ public 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);
+ }
+
+ /**
+ * No default command.
+ */
+ @Override
+ public void initDefaultCommand() {
+ }
+
+ /**
+ * Extend both solenoids to shoot.
+ */
+ public void extendBoth() {
+ m_piston1.set(DoubleSolenoid.Value.kForward);
+ m_piston2.set(DoubleSolenoid.Value.kForward);
+ }
+
+ /**
+ * Retract both solenoids to prepare to shoot.
+ */
+ public void retractBoth() {
+ m_piston1.set(DoubleSolenoid.Value.kReverse);
+ m_piston2.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Extend solenoid 1 to shoot.
+ */
+ public void extend1() {
+ m_piston1.set(DoubleSolenoid.Value.kForward);
+ }
+
+ /**
+ * Retract solenoid 1 to prepare to shoot.
+ */
+ public void retract1() {
+ m_piston1.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Extend solenoid 2 to shoot.
+ */
+ public void extend2() {
+ m_piston2.set(DoubleSolenoid.Value.kReverse);
+ }
+
+ /**
+ * Retract solenoid 2 to prepare to shoot.
+ */
+ public void retract2() {
+ m_piston2.set(DoubleSolenoid.Value.kForward);
+ }
+
+ /**
+ * 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.
+ */
+ public void off1() {
+ m_piston1.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * Turns off the piston2 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.
+ */
+ public void off2() {
+ m_piston2.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * Release the latch so that we can shoot.
+ */
+ public void unlatch() {
+ m_latchPiston.set(true);
+ }
+
+ /**
+ * Latch so that pressure can build up and we aren't limited by air flow.
+ */
+ public void latch() {
+ m_latchPiston.set(false);
+ }
+
+ /**
+ * Toggles the latch postions.
+ */
+ public void toggleLatchPosition() {
+ m_latchPiston.set(!m_latchPiston.get());
+ }
+
+ /**
+ * Is Piston 1 extended (after shooting).
+ *
+ * @return Whether or not piston 1 is fully extended.
+ */
+ public boolean piston1IsExtended() {
+ return !m_piston1ReedSwitchFront.get();
+ }
+
+ /**
+ * Is Piston 1 retracted (before shooting).
+ *
+ * @return Whether or not piston 1 is fully retracted.
+ */
+ public boolean piston1IsRetracted() {
+ return !m_piston1ReedSwitchBack.get();
+ }
+
+ /**
+ * 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.
+ */
+ public void offBoth() {
+ m_piston1.set(DoubleSolenoid.Value.kOff);
+ m_piston2.set(DoubleSolenoid.Value.kOff);
+ }
+
+ /**
+ * Return whether the goal is hot as read by the banner sensor.
+ *
+ * <p>NOTE: doesn't work in simulation.
+ *
+ * @return Whether or not the goal is hot
+ */
+ public boolean goalIsHot() {
+ return m_hotGoalSensor.get();
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
new file mode 100644
index 0000000..c20cc47
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.pacgoat.triggers;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.buttons.Trigger;
+
+/**
+ * A custom button that is triggered when two buttons on a Joystick are
+ * simultaneously pressed.
+ */
+public class DoubleButton extends Trigger {
+ private Joystick m_joy;
+ private int m_button1;
+ private int m_button2;
+
+ public DoubleButton(Joystick joy, int button1, int button2) {
+ this.m_joy = joy;
+ this.m_button1 = button1;
+ this.m_button2 = button2;
+ }
+
+ @Override
+ public boolean get() {
+ return m_joy.getRawButton(m_button1) && m_joy.getRawButton(m_button2);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
new file mode 100644
index 0000000..0af1885
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.potentiometerpid;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.Joystick;
+
+/**
+ * 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.
+ */
+public class Robot extends IterativeRobot {
+ private static final int kPotChannel = 1;
+ private static final int kMotorChannel = 7;
+ private static final int kJoystickChannel = 0;
+
+ // bottom, middle, and top elevator setpoints
+ private static final double[] kSetPoints = {1.0, 2.6, 4.3};
+
+ // proportional, integral, and derivative speed constants; motor inverted
+ // DANGER: when tuning PID constants, high/inappropriate values for kP, kI,
+ // and kD may cause dangerous, uncontrollable, or undesired behavior!
+ // these may need to be positive for a non-inverted motor
+ private static final double kP = -5.0;
+ private static final double kI = -0.02;
+ private static final double kD = -2.0;
+
+ private PIDController m_pidController;
+ private AnalogInput m_potentiometer;
+ private SpeedController m_elevatorMotor;
+ private Joystick m_joystick;
+
+ private int m_index = 0;
+ private boolean m_previousButtonValue = false;
+
+ @Override
+ public void robotInit() {
+ m_potentiometer = new AnalogInput(kPotChannel);
+ m_elevatorMotor = new Spark(kMotorChannel);
+ m_joystick = new Joystick(kJoystickChannel);
+
+ m_pidController
+ = new PIDController(kP, kI, kD, m_potentiometer, m_elevatorMotor);
+ m_pidController.setInputRange(0, 5);
+ }
+
+ @Override
+ public void teleopInit() {
+ m_pidController.enable();
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ // when the button is pressed once, the selected elevator setpoint
+ // is incremented
+ boolean currentButtonValue = m_joystick.getTrigger();
+ if (currentButtonValue && !m_previousButtonValue) {
+ // index of the elevator setpoint wraps around.
+ m_index = (m_index + 1) % kSetPoints.length;
+ }
+ m_previousButtonValue = currentButtonValue;
+
+ m_pidController.setSetpoint(kSetPoints[m_index]);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java
new file mode 100644
index 0000000..db649ec
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.quickvision;
+
+import edu.wpi.first.wpilibj.CameraServer;
+import edu.wpi.first.wpilibj.IterativeRobot;
+
+/**
+ * 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.
+ */
+public class Robot extends IterativeRobot {
+ @Override
+ public void robotInit() {
+ CameraServer.getInstance().startAutomaticCapture();
+ }
+
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
new file mode 100755
index 0000000..9808992
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.tankdrive;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * This is a demo program showing the use of the RobotDrive class, specifically
+ * it contains the code necessary to operate a robot with tank drive.
+ */
+public class Robot extends IterativeRobot {
+ private DifferentialDrive m_myRobot;
+ private Joystick m_leftStick;
+ private Joystick m_rightStick;
+
+ @Override
+ public void robotInit() {
+ m_myRobot = new DifferentialDrive(new Spark(0), new Spark(1));
+ m_leftStick = new Joystick(0);
+ m_rightStick = new Joystick(1);
+ }
+
+ @Override
+ public void teleopPeriodic() {
+ m_myRobot.tankDrive(m_leftStick.getY(), m_rightStick.getY());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
new file mode 100644
index 0000000..5ac6341
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ultrasonic;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * This is a sample program demonstrating how to use an ultrasonic sensor and
+ * proportional control to maintain a set distance from an object.
+ */
+
+public class Robot extends IterativeRobot {
+ // distance in inches the robot wants to stay from an object
+ private static final double kHoldDistance = 12.0;
+
+ // factor to convert sensor values to a distance in inches
+ private static final double kValueToInches = 0.125;
+
+ // proportional speed constant
+ private static final double kP = 0.05;
+
+ private static final int kLeftMotorPort = 0;
+ private static final int kRightMotorPort = 1;
+ private static final int kUltrasonicPort = 0;
+
+ private AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
+ private DifferentialDrive m_robotDrive
+ = new DifferentialDrive(new Spark(kLeftMotorPort),
+ new Spark(kRightMotorPort));
+
+ /**
+ * Tells the robot to drive to a set distance (in inches) from an object
+ * using proportional control.
+ */
+ public void teleopPeriodic() {
+ // 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);
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
new file mode 100644
index 0000000..066150b
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.ultrasonicpid;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+
+/**
+ * This is a sample program to demonstrate the use of a PIDController with an
+ * ultrasonic sensor to reach and maintain a set distance from an object.
+ */
+public class Robot extends IterativeRobot {
+ // distance in inches the robot wants to stay from an object
+ private static final double kHoldDistance = 12.0;
+
+ // maximum distance in inches we expect the robot to see
+ private static final double kMaxDistance = 24.0;
+
+ // factor to convert sensor values to a distance in inches
+ private static final double kValueToInches = 0.125;
+
+ // proportional speed constant
+ private static final double kP = 7.0;
+
+ // integral speed constant
+ private static final double kI = 0.018;
+
+ // derivative speed constant
+ private static final double kD = 1.5;
+
+ private static final int kLeftMotorPort = 0;
+ private static final int kRightMotorPort = 1;
+ private static final int kUltrasonicPort = 0;
+
+ private AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
+ private DifferentialDrive m_robotDrive
+ = new DifferentialDrive(new Spark(kLeftMotorPort),
+ new Spark(kRightMotorPort));
+ private PIDController m_pidController
+ = new PIDController(kP, kI, kD, m_ultrasonic, new MyPidOutput());
+
+ /**
+ * Drives the robot a set distance from an object using PID control and the
+ * ultrasonic sensor.
+ */
+ @Override
+ public void teleopInit() {
+ // 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, kMaxDistance * kValueToInches);
+ // Set setpoint of the pid controller
+ m_pidController.setSetpoint(kHoldDistance * kValueToInches);
+ m_pidController.enable(); // begin PID control
+ }
+
+ private class MyPidOutput implements PIDOutput {
+ @Override
+ public void pidWrite(double output) {
+ m_robotDrive.arcadeDrive(output, 0);
+ }
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java
new file mode 100644
index 0000000..b1833f2
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/OI.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.commandbased;
+
+/**
+ * This class is the glue that binds the controls on the physical operator
+ * interface to the commands and command groups that allow control of the robot.
+ */
+public class OI {
+ //// CREATING BUTTONS
+ // One type of button is a joystick button which is any button on a
+ //// joystick.
+ // You create one by telling it which joystick it's on and which button
+ // number it is.
+ // Joystick stick = new Joystick(port);
+ // Button button = new JoystickButton(stick, buttonNumber);
+
+ // There are a few additional built in buttons you can use. Additionally,
+ // by subclassing Button you can create custom triggers and bind those to
+ // commands the same as any other Button.
+
+ //// TRIGGERING COMMANDS WITH BUTTONS
+ // Once you have a button, it's trivial to bind it to a button in one of
+ // three ways:
+
+ // Start the command when the button is pressed and let it run the command
+ // until it is finished as determined by it's isFinished method.
+ // button.whenPressed(new ExampleCommand());
+
+ // Run the command while the button is being held down and interrupt it once
+ // the button is released.
+ // button.whileHeld(new ExampleCommand());
+
+ // Start the command when the button is released and let it run the command
+ // until it is finished as determined by it's isFinished method.
+ // button.whenReleased(new ExampleCommand());
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
new file mode 100644
index 0000000..8c69039
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.commandbased;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.templates.commandbased.commands.ExampleCommand;
+import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the TimedRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.properties file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ public static final ExampleSubsystem kExampleSubsystem
+ = new ExampleSubsystem();
+ public static OI m_oi;
+
+ Command m_autonomousCommand;
+ SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+ /**
+ * This function is run when the robot is first started up and should be
+ * used for any initialization code.
+ */
+ @Override
+ public void robotInit() {
+ m_oi = new OI();
+ m_chooser.addDefault("Default Auto", new ExampleCommand());
+ // chooser.addObject("My Auto", new MyAutoCommand());
+ SmartDashboard.putData("Auto mode", m_chooser);
+ }
+
+ /**
+ * 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.
+ */
+ @Override
+ public void disabledInit() {
+
+ }
+
+ @Override
+ public void disabledPeriodic() {
+ 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
+ *
+ * <p>You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons
+ * to the switch structure below with additional strings & commands.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autonomousCommand = m_chooser.getSelected();
+
+ /*
+ * String autoSelected = SmartDashboard.getString("Auto Selector",
+ * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
+ * = new MyAutoCommand(); break; case "Default Auto": default:
+ * autonomousCommand = new ExampleCommand(); break; }
+ */
+
+ // schedule the autonomous command (example)
+ if (m_autonomousCommand != null) {
+ m_autonomousCommand.start();
+ }
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ Scheduler.getInstance().run();
+ }
+
+ @Override
+ public void 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 != null) {
+ m_autonomousCommand.cancel();
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ Scheduler.getInstance().run();
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java
new file mode 100644
index 0000000..709e7b2
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotMap.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.commandbased;
+
+/**
+ * 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.
+ */
+public class RobotMap {
+ // For example to map the left and right motors, you could define the
+ // following variables to use with your drivetrain subsystem.
+ // public static int leftMotor = 1;
+ // public static int rightMotor = 2;
+
+ // If you are using multiple modules, make sure to define both the port
+ // number and the module. For example you with a rangefinder:
+ // public static int rangefinderPort = 1;
+ // public static int rangefinderModule = 1;
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
new file mode 100644
index 0000000..115d686
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
@@ -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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.commandbased.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.templates.commandbased.Robot;
+
+/**
+ * An example command. You can replace me with your own command.
+ */
+public class ExampleCommand extends Command {
+ public ExampleCommand() {
+ // Use requires() here to declare subsystem dependencies
+ requires(Robot.kExampleSubsystem);
+ }
+
+ // Called just before this Command runs the first time
+ @Override
+ protected void initialize() {
+ }
+
+ // Called repeatedly when this Command is scheduled to run
+ @Override
+ protected void execute() {
+ }
+
+ // Make this return true when this Command no longer needs to run execute()
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ // Called once after isFinished returns true
+ @Override
+ protected void end() {
+ }
+
+ // Called when another command which requires one or more of the same
+ // subsystems is scheduled to run
+ @Override
+ protected void interrupted() {
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
new file mode 100644
index 0000000..7dcafbe
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.commandbased.subsystems;
+
+import edu.wpi.first.wpilibj.command.Subsystem;
+
+/**
+ * An example subsystem. You can replace me with your own Subsystem.
+ */
+public class ExampleSubsystem extends Subsystem {
+ // Put methods for controlling this subsystem
+ // here. Call these from Commands.
+
+ public void initDefaultCommand() {
+ // Set the default command for a subsystem here.
+ // setDefaultCommand(new MySpecialCommand());
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
new file mode 100644
index 0000000..e6e1027
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/iterative/Robot.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.iterative;
+
+import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.properties file in the
+ * project.
+ */
+public class Robot extends IterativeRobot {
+ private static final String kDefaultAuto = "Default";
+ private static final String kCustomAuto = "My Auto";
+ private String m_autoSelected;
+ private SendableChooser<String> m_chooser = new SendableChooser<>();
+
+ /**
+ * This function is run when the robot is first started up and should be
+ * used for any initialization code.
+ */
+ @Override
+ public void robotInit() {
+ m_chooser.addDefault("Default Auto", kDefaultAuto);
+ m_chooser.addObject("My Auto", kCustomAuto);
+ SmartDashboard.putData("Auto choices", 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
+ *
+ * <p>You can add additional auto modes by adding additional comparisons to
+ * the switch structure below with additional strings. If using the
+ * SendableChooser make sure to add them to the chooser code above as well.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autoSelected = m_chooser.getSelected();
+ // autoSelected = SmartDashboard.getString("Auto Selector",
+ // defaultAuto);
+ System.out.println("Auto selected: " + m_autoSelected);
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ switch (m_autoSelected) {
+ case kCustomAuto:
+ // Put custom auto code here
+ break;
+ case kDefaultAuto:
+ default:
+ // Put default auto code here
+ break;
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java
new file mode 100644
index 0000000..fc9f167
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/sample/Robot.java
@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.sample;
+
+import edu.wpi.first.wpilibj.SampleRobot;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * This is a demo program showing the use of the RobotDrive 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.
+ *
+ * <p>The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the SampleRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.properties file in the
+ * project.
+ *
+ * <p>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 IterativeRobot or
+ * Command-Based instead if you're new.
+ */
+public class Robot extends SampleRobot {
+ private static final String kDefaultAuto = "Default";
+ private static final String kCustomAuto = "My Auto";
+
+ private DifferentialDrive m_robotDrive
+ = new DifferentialDrive(new Spark(0), new Spark(1));
+ private Joystick m_stick = new Joystick(0);
+ private SendableChooser<String> m_chooser = new SendableChooser<>();
+
+ public Robot() {
+ m_robotDrive.setExpiration(0.1);
+ }
+
+ @Override
+ public void robotInit() {
+ m_chooser.addDefault("Default Auto", kDefaultAuto);
+ m_chooser.addObject("My Auto", kCustomAuto);
+ 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
+ *
+ * <p>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.
+ *
+ * <p>If you wanted to run a similar autonomous mode with an IterativeRobot
+ * you would write:
+ *
+ * <blockquote><pre>{@code
+ * Timer timer = new Timer();
+ *
+ * // This function is run once each time the robot enters autonomous mode
+ * public void autonomousInit() {
+ * timer.reset();
+ * timer.start();
+ * }
+ *
+ * // This function is called periodically during autonomous
+ * public void autonomousPeriodic() {
+ * // Drive for 2 seconds
+ * if (timer.get() < 2.0) {
+ * myRobot.drive(-0.5, 0.0); // drive forwards half speed
+ * } else if (timer.get() < 5.0) {
+ * myRobot.drive(-1.0, 0.0); // drive forwards full speed
+ * } else {
+ * myRobot.drive(0.0, 0.0); // stop robot
+ * }
+ * }
+ * }</pre></blockquote>
+ */
+ @Override
+ public void autonomous() {
+ String autoSelected = m_chooser.getSelected();
+ // String autoSelected = SmartDashboard.getString("Auto Selector",
+ // defaultAuto);
+ System.out.println("Auto selected: " + autoSelected);
+
+ // 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);
+
+ switch (autoSelected) {
+ case kCustomAuto:
+ // Spin at half speed for two seconds
+ m_robotDrive.arcadeDrive(0.0, 0.5);
+ Timer.delay(2.0);
+
+ // Stop robot
+ m_robotDrive.arcadeDrive(0.0, 0.0);
+ break;
+ case kDefaultAuto:
+ default:
+ // Drive forwards for two seconds
+ m_robotDrive.arcadeDrive(-0.5, 0.0);
+ Timer.delay(2.0);
+
+ // Stop robot
+ m_robotDrive.arcadeDrive(0.0, 0.0);
+ break;
+ }
+ }
+
+ /**
+ * Runs the motors with arcade steering.
+ *
+ * <p>If you wanted to run a similar teleoperated mode with an IterativeRobot
+ * you would write:
+ *
+ * <blockquote><pre>{@code
+ * // This function is called periodically during operator control
+ * public void teleopPeriodic() {
+ * myRobot.arcadeDrive(stick);
+ * }
+ * }</pre></blockquote>
+ */
+ @Override
+ public void operatorControl() {
+ m_robotDrive.setSafetyEnabled(true);
+ while (isOperatorControl() && isEnabled()) {
+ // Drive arcade style
+ m_robotDrive.arcadeDrive(-m_stick.getY(), m_stick.getX());
+
+ // The motors will be updated every 5ms
+ Timer.delay(0.005);
+ }
+ }
+
+ /**
+ * Runs during test mode.
+ */
+ @Override
+ public void test() {
+ }
+}
diff --git a/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
new file mode 100644
index 0000000..ff869fd
--- /dev/null
+++ b/third_party/allwpilib_2018/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.templates.timed;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to each mode, as described in the IterativeRobot
+ * documentation. If you change the name of this class or the package after
+ * creating this project, you must also update the build.properties file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+ private static final String kDefaultAuto = "Default";
+ private static final String kCustomAuto = "My Auto";
+ private String m_autoSelected;
+ private SendableChooser<String> m_chooser = new SendableChooser<>();
+
+ /**
+ * This function is run when the robot is first started up and should be
+ * used for any initialization code.
+ */
+ @Override
+ public void robotInit() {
+ m_chooser.addDefault("Default Auto", kDefaultAuto);
+ m_chooser.addObject("My Auto", kCustomAuto);
+ SmartDashboard.putData("Auto choices", 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
+ *
+ * <p>You can add additional auto modes by adding additional comparisons to
+ * the switch structure below with additional strings. If using the
+ * SendableChooser make sure to add them to the chooser code above as well.
+ */
+ @Override
+ public void autonomousInit() {
+ m_autoSelected = m_chooser.getSelected();
+ // m_autoSelected = SmartDashboard.getString("Auto Selector",
+ // kDefaultAuto);
+ System.out.println("Auto selected: " + m_autoSelected);
+ }
+
+ /**
+ * This function is called periodically during autonomous.
+ */
+ @Override
+ public void autonomousPeriodic() {
+ switch (m_autoSelected) {
+ case kCustomAuto:
+ // Put custom auto code here
+ break;
+ case kDefaultAuto:
+ default:
+ // Put default auto code here
+ break;
+ }
+ }
+
+ /**
+ * This function is called periodically during operator control.
+ */
+ @Override
+ public void teleopPeriodic() {
+ }
+
+ /**
+ * This function is called periodically during test mode.
+ */
+ @Override
+ public void testPeriodic() {
+ }
+}