New 2018 and later build setup (#1001)

wpilibcIntegrationTests/src/main/native/cpp/AnalogLoopTest.cpp

@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogInput.h"
+#include "AnalogOutput.h"
+#include "AnalogTrigger.h"
+#include "Counter.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const double kDelayTime = 0.01;
+
+/**
+ * A fixture with an analog input and an analog output wired together
+ */
+class AnalogLoopTest : public testing::Test {
+ protected:
+  AnalogInput* m_input;
+  AnalogOutput* m_output;
+
+  void SetUp() override {
+    m_input = new AnalogInput(TestBench::kFakeAnalogOutputChannel);
+    m_output = new AnalogOutput(TestBench::kAnalogOutputChannel);
+  }
+
+  void TearDown() override {
+    delete m_input;
+    delete m_output;
+  }
+};
+
+/**
+ * Test analog inputs and outputs by setting one and making sure the other
+ * matches.
+ */
+TEST_F(AnalogLoopTest, AnalogInputWorks) {
+  // Set the output voltage and check if the input measures the same voltage
+  for (int32_t i = 0; i < 50; i++) {
+    m_output->SetVoltage(i / 10.0);
+
+    Wait(kDelayTime);
+
+    EXPECT_NEAR(m_output->GetVoltage(), m_input->GetVoltage(), 0.01);
+  }
+}
+
+/**
+ * Test if we can use an analog trigger to  check if the output is within a
+ * range correctly.
+ */
+TEST_F(AnalogLoopTest, AnalogTriggerWorks) {
+  AnalogTrigger trigger(m_input);
+  trigger.SetLimitsVoltage(2.0, 3.0);
+
+  m_output->SetVoltage(1.0);
+  Wait(kDelayTime);
+
+  EXPECT_FALSE(trigger.GetInWindow())
+      << "Analog trigger is in the window (2V, 3V)";
+  EXPECT_FALSE(trigger.GetTriggerState()) << "Analog trigger is on";
+
+  m_output->SetVoltage(2.5);
+  Wait(kDelayTime);
+
+  EXPECT_TRUE(trigger.GetInWindow())
+      << "Analog trigger is not in the window (2V, 3V)";
+  EXPECT_FALSE(trigger.GetTriggerState()) << "Analog trigger is on";
+
+  m_output->SetVoltage(4.0);
+  Wait(kDelayTime);
+
+  EXPECT_FALSE(trigger.GetInWindow())
+      << "Analog trigger is in the window (2V, 3V)";
+  EXPECT_TRUE(trigger.GetTriggerState()) << "Analog trigger is not on";
+}
+
+/**
+ * Test if we can count the right number of ticks from an analog trigger with
+ * a counter.
+ */
+TEST_F(AnalogLoopTest, AnalogTriggerCounterWorks) {
+  AnalogTrigger trigger(m_input);
+  trigger.SetLimitsVoltage(2.0, 3.0);
+
+  Counter counter(trigger);
+
+  // Turn the analog output low and high 50 times
+  for (int32_t i = 0; i < 50; i++) {
+    m_output->SetVoltage(1.0);
+    Wait(kDelayTime);
+    m_output->SetVoltage(4.0);
+    Wait(kDelayTime);
+  }
+
+  // The counter should be 50
+  EXPECT_EQ(50, counter.Get())
+      << "Analog trigger counter did not count 50 ticks";
+}
+
+static void InterruptHandler(uint32_t interruptAssertedMask, void* param) {
+  *reinterpret_cast<int32_t*>(param) = 12345;
+}
+
+TEST_F(AnalogLoopTest, AsynchronusInterruptWorks) {
+  int32_t param = 0;
+  AnalogTrigger trigger(m_input);
+  trigger.SetLimitsVoltage(2.0, 3.0);
+
+  // Given an interrupt handler that sets an int32_t to 12345
+  std::shared_ptr<AnalogTriggerOutput> triggerOutput =
+      trigger.CreateOutput(AnalogTriggerType::kState);
+  triggerOutput->RequestInterrupts(InterruptHandler, &param);
+  triggerOutput->EnableInterrupts();
+
+  // If the analog output moves from below to above the window
+  m_output->SetVoltage(0.0);
+  Wait(kDelayTime);
+  m_output->SetVoltage(5.0);
+  triggerOutput->CancelInterrupts();
+
+  // Then the int32_t should be 12345
+  Wait(kDelayTime);
+  EXPECT_EQ(12345, param) << "The interrupt did not run.";
+}

wpilibcIntegrationTests/src/main/native/cpp/AnalogPotentiometerTest.cpp

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogPotentiometer.h"  // NOLINT(build/include_order)
+
+#include "AnalogOutput.h"
+#include "RobotController.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const double kScale = 270.0;
+static const double kAngle = 180.0;
+
+class AnalogPotentiometerTest : public testing::Test {
+ protected:
+  AnalogOutput* m_fakePot;
+  AnalogPotentiometer* m_pot;
+
+  void SetUp() override {
+    m_fakePot = new AnalogOutput(TestBench::kAnalogOutputChannel);
+    m_pot =
+        new AnalogPotentiometer(TestBench::kFakeAnalogOutputChannel, kScale);
+  }
+
+  void TearDown() override {
+    delete m_fakePot;
+    delete m_pot;
+  }
+};
+
+TEST_F(AnalogPotentiometerTest, TestInitialSettings) {
+  m_fakePot->SetVoltage(0.0);
+  Wait(0.1);
+  EXPECT_NEAR(0.0, m_pot->Get(), 5.0)
+      << "The potentiometer did not initialize to 0.";
+}
+
+TEST_F(AnalogPotentiometerTest, TestRangeValues) {
+  m_fakePot->SetVoltage(kAngle / kScale * RobotController::GetVoltage5V());
+  Wait(0.1);
+  EXPECT_NEAR(kAngle, m_pot->Get(), 2.0)
+      << "The potentiometer did not measure the correct angle.";
+}

wpilibcIntegrationTests/src/main/native/cpp/BuiltInAccelerometerTest.cpp

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "BuiltInAccelerometer.h"  // NOLINT(build/include_order)
+
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kAccelerationTolerance = 0.1;
+/**
+ * There's not much we can automatically test with the on-board accelerometer,
+ * but checking for gravity is probably good enough to tell that it's working.
+ */
+TEST(BuiltInAccelerometerTest, Accelerometer) {
+  BuiltInAccelerometer accelerometer;
+
+  /* The testbench sometimes shakes a little from a previous test.  Give it
+          some time. */
+  Wait(1.0);
+
+  ASSERT_NEAR(0.0, accelerometer.GetX(), kAccelerationTolerance);
+  ASSERT_NEAR(1.0, accelerometer.GetY(), kAccelerationTolerance);
+  ASSERT_NEAR(0.0, accelerometer.GetZ(), kAccelerationTolerance);
+}

wpilibcIntegrationTests/src/main/native/cpp/CircularBufferTest.cpp

@@ -0,0 +1,211 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "circular_buffer.h"  // NOLINT(build/include_order)
+
+#include <array>
+
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const std::array<double, 10> values = {
+    751.848, 766.366, 342.657, 234.252, 716.126,
+    132.344, 445.697, 22.727,  421.125, 799.913};
+
+static const std::array<double, 8> pushFrontOut = {
+    799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657};
+
+static const std::array<double, 8> pushBackOut = {
+    342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913};
+
+TEST(CircularBufferTest, PushFrontTest) {
+  circular_buffer<double> queue(8);
+
+  for (auto& value : values) {
+    queue.push_front(value);
+  }
+
+  for (size_t i = 0; i < pushFrontOut.size(); i++) {
+    EXPECT_EQ(pushFrontOut[i], queue[i]);
+  }
+}
+
+TEST(CircularBufferTest, PushBackTest) {
+  circular_buffer<double> queue(8);
+
+  for (auto& value : values) {
+    queue.push_back(value);
+  }
+
+  for (size_t i = 0; i < pushBackOut.size(); i++) {
+    EXPECT_EQ(pushBackOut[i], queue[i]);
+  }
+}
+
+TEST(CircularBufferTest, PushPopTest) {
+  circular_buffer<double> queue(3);
+
+  // Insert three elements into the buffer
+  queue.push_back(1.0);
+  queue.push_back(2.0);
+  queue.push_back(3.0);
+
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+  EXPECT_EQ(3.0, queue[2]);
+
+  /*
+   * The buffer is full now, so pushing subsequent elements will overwrite the
+   * front-most elements.
+   */
+
+  queue.push_back(4.0);  // Overwrite 1 with 4
+
+  // The buffer now contains 2, 3 and 4
+  EXPECT_EQ(2.0, queue[0]);
+  EXPECT_EQ(3.0, queue[1]);
+  EXPECT_EQ(4.0, queue[2]);
+
+  queue.push_back(5.0);  // Overwrite 2 with 5
+
+  // The buffer now contains 3, 4 and 5
+  EXPECT_EQ(3.0, queue[0]);
+  EXPECT_EQ(4.0, queue[1]);
+  EXPECT_EQ(5.0, queue[2]);
+
+  EXPECT_EQ(5.0, queue.pop_back());  // 5 is removed
+
+  // The buffer now contains 3 and 4
+  EXPECT_EQ(3.0, queue[0]);
+  EXPECT_EQ(4.0, queue[1]);
+
+  EXPECT_EQ(3.0, queue.pop_front());  // 3 is removed
+
+  // Leaving only one element with value == 4
+  EXPECT_EQ(4.0, queue[0]);
+}
+
+TEST(CircularBufferTest, ResetTest) {
+  circular_buffer<double> queue(5);
+
+  for (size_t i = 1; i < 6; i++) {
+    queue.push_back(i);
+  }
+
+  queue.reset();
+
+  for (size_t i = 0; i < 5; i++) {
+    EXPECT_EQ(0.0, queue[i]);
+  }
+}
+
+TEST(CircularBufferTest, ResizeTest) {
+  circular_buffer<double> queue(5);
+
+  /* Buffer contains {1, 2, 3, _, _}
+   *                  ^ front
+   */
+  queue.push_back(1.0);
+  queue.push_back(2.0);
+  queue.push_back(3.0);
+
+  queue.resize(2);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.resize(5);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.reset();
+
+  /* Buffer contains {_, 1, 2, 3, _}
+   *                     ^ front
+   */
+  queue.push_back(0.0);
+  queue.push_back(1.0);
+  queue.push_back(2.0);
+  queue.push_back(3.0);
+  queue.pop_front();
+
+  queue.resize(2);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.resize(5);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.reset();
+
+  /* Buffer contains {_, _, 1, 2, 3}
+   *                        ^ front
+   */
+  queue.push_back(0.0);
+  queue.push_back(0.0);
+  queue.push_back(1.0);
+  queue.push_back(2.0);
+  queue.push_back(3.0);
+  queue.pop_front();
+  queue.pop_front();
+
+  queue.resize(2);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.resize(5);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.reset();
+
+  /* Buffer contains {3, _, _, 1, 2}
+   *                           ^ front
+   */
+  queue.push_back(3.0);
+  queue.push_front(2.0);
+  queue.push_front(1.0);
+
+  queue.resize(2);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.resize(5);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.reset();
+
+  /* Buffer contains {2, 3, _, _, 1}
+   *                              ^ front
+   */
+  queue.push_back(2.0);
+  queue.push_back(3.0);
+  queue.push_front(1.0);
+
+  queue.resize(2);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  queue.resize(5);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+
+  // Test push_back() after resize
+  queue.push_back(3.0);
+  EXPECT_EQ(1.0, queue[0]);
+  EXPECT_EQ(2.0, queue[1]);
+  EXPECT_EQ(3.0, queue[2]);
+
+  // Test push_front() after resize
+  queue.push_front(4.0);
+  EXPECT_EQ(4.0, queue[0]);
+  EXPECT_EQ(1.0, queue[1]);
+  EXPECT_EQ(2.0, queue[2]);
+  EXPECT_EQ(3.0, queue[3]);
+}

wpilibcIntegrationTests/src/main/native/cpp/CounterTest.cpp

@@ -0,0 +1,176 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Counter.h"  // NOLINT(build/include_order)
+
+#include "Jaguar.h"
+#include "Talon.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "Victor.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const double kMotorDelay = 2.5;
+
+static const double kMaxPeriod = 2.0;
+
+class CounterTest : public testing::Test {
+ protected:
+  Counter* m_talonCounter;
+  Counter* m_victorCounter;
+  Counter* m_jaguarCounter;
+  Talon* m_talon;
+  Victor* m_victor;
+  Jaguar* m_jaguar;
+
+  void SetUp() override {
+    m_talonCounter = new Counter(TestBench::kTalonEncoderChannelA);
+    m_victorCounter = new Counter(TestBench::kVictorEncoderChannelA);
+    m_jaguarCounter = new Counter(TestBench::kJaguarEncoderChannelA);
+    m_victor = new Victor(TestBench::kVictorChannel);
+    m_talon = new Talon(TestBench::kTalonChannel);
+    m_jaguar = new Jaguar(TestBench::kJaguarChannel);
+  }
+
+  void TearDown() override {
+    delete m_talonCounter;
+    delete m_victorCounter;
+    delete m_jaguarCounter;
+    delete m_victor;
+    delete m_talon;
+    delete m_jaguar;
+  }
+
+  void Reset() {
+    m_talonCounter->Reset();
+    m_victorCounter->Reset();
+    m_jaguarCounter->Reset();
+    m_talon->Set(0.0);
+    m_victor->Set(0.0);
+    m_jaguar->Set(0.0);
+  }
+};
+
+/**
+ * Tests the counter by moving the motor and determining if the
+ * counter is counting.
+ */
+TEST_F(CounterTest, CountTalon) {
+  Reset();
+
+  /* Run the motor forward and determine if the counter is counting. */
+  m_talon->Set(1.0);
+  Wait(0.5);
+
+  EXPECT_NE(0.0, m_talonCounter->Get()) << "The counter did not count (talon)";
+
+  /* Set the motor to 0 and determine if the counter resets to 0. */
+  m_talon->Set(0.0);
+  Wait(0.5);
+  m_talonCounter->Reset();
+
+  EXPECT_FLOAT_EQ(0.0, m_talonCounter->Get())
+      << "The counter did not restart to 0 (talon)";
+}
+
+TEST_F(CounterTest, CountVictor) {
+  Reset();
+
+  /* Run the motor forward and determine if the counter is counting. */
+  m_victor->Set(1.0);
+  Wait(0.5);
+
+  EXPECT_NE(0.0, m_victorCounter->Get())
+      << "The counter did not count (victor)";
+
+  /* Set the motor to 0 and determine if the counter resets to 0. */
+  m_victor->Set(0.0);
+  Wait(0.5);
+  m_victorCounter->Reset();
+
+  EXPECT_FLOAT_EQ(0.0, m_victorCounter->Get())
+      << "The counter did not restart to 0 (jaguar)";
+}
+
+TEST_F(CounterTest, CountJaguar) {
+  Reset();
+
+  /* Run the motor forward and determine if the counter is counting. */
+  m_jaguar->Set(1.0);
+  Wait(0.5);
+
+  EXPECT_NE(0.0, m_jaguarCounter->Get())
+      << "The counter did not count (jaguar)";
+
+  /* Set the motor to 0 and determine if the counter resets to 0. */
+  m_jaguar->Set(0.0);
+  Wait(0.5);
+  m_jaguarCounter->Reset();
+
+  EXPECT_FLOAT_EQ(0.0, m_jaguarCounter->Get())
+      << "The counter did not restart to 0 (jaguar)";
+}
+
+/**
+ * Tests the GetStopped and SetMaxPeriod methods by setting the Max Period and
+ * getting the value after a period of time.
+ */
+TEST_F(CounterTest, TalonGetStopped) {
+  Reset();
+
+  /* Set the Max Period of the counter and run the motor */
+  m_talonCounter->SetMaxPeriod(kMaxPeriod);
+  m_talon->Set(1.0);
+  Wait(0.5);
+
+  EXPECT_FALSE(m_talonCounter->GetStopped()) << "The counter did not count.";
+
+  /* Stop the motor and wait until the Max Period is exceeded */
+  m_talon->Set(0.0);
+  Wait(kMotorDelay);
+
+  EXPECT_TRUE(m_talonCounter->GetStopped())
+      << "The counter did not stop counting.";
+}
+
+TEST_F(CounterTest, VictorGetStopped) {
+  Reset();
+
+  /* Set the Max Period of the counter and run the motor */
+  m_victorCounter->SetMaxPeriod(kMaxPeriod);
+  m_victor->Set(1.0);
+  Wait(0.5);
+
+  EXPECT_FALSE(m_victorCounter->GetStopped()) << "The counter did not count.";
+
+  /* Stop the motor and wait until the Max Period is exceeded */
+  m_victor->Set(0.0);
+  Wait(kMotorDelay);
+
+  EXPECT_TRUE(m_victorCounter->GetStopped())
+      << "The counter did not stop counting.";
+}
+
+TEST_F(CounterTest, JaguarGetStopped) {
+  Reset();
+
+  /* Set the Max Period of the counter and run the motor */
+  m_jaguarCounter->SetMaxPeriod(kMaxPeriod);
+  m_jaguar->Set(1.0);
+  Wait(0.5);
+
+  EXPECT_FALSE(m_jaguarCounter->GetStopped()) << "The counter did not count.";
+
+  /* Stop the motor and wait until the Max Period is exceeded */
+  m_jaguar->Set(0.0);
+  Wait(kMotorDelay);
+
+  EXPECT_TRUE(m_jaguarCounter->GetStopped())
+      << "The counter did not stop counting.";
+}

wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp

@@ -0,0 +1,188 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "DigitalInput.h"  // NOLINT(build/include_order)
+
+#include "DigitalOutput.h"  // NOLINT(build/include_order)
+
+#include "Counter.h"
+#include "InterruptableSensorBase.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const double kCounterTime = 0.001;
+
+static const double kDelayTime = 0.1;
+
+static const double kSynchronousInterruptTime = 2.0;
+static const double kSynchronousInterruptTimeTolerance = 0.01;
+
+/**
+ * A fixture with a digital input and a digital output physically wired
+ * together.
+ */
+class DIOLoopTest : public testing::Test {
+ protected:
+  DigitalInput* m_input;
+  DigitalOutput* m_output;
+
+  void SetUp() override {
+    m_input = new DigitalInput(TestBench::kLoop1InputChannel);
+    m_output = new DigitalOutput(TestBench::kLoop1OutputChannel);
+  }
+
+  void TearDown() override {
+    delete m_input;
+    delete m_output;
+  }
+
+  void Reset() { m_output->Set(false); }
+};
+
+/**
+ * Test the DigitalInput and DigitalOutput classes by setting the output and
+ * reading the input.
+ */
+TEST_F(DIOLoopTest, Loop) {
+  Reset();
+
+  m_output->Set(false);
+  Wait(kDelayTime);
+  EXPECT_FALSE(m_input->Get()) << "The digital output was turned off, but "
+                               << "the digital input is on.";
+
+  m_output->Set(true);
+  Wait(kDelayTime);
+  EXPECT_TRUE(m_input->Get()) << "The digital output was turned on, but "
+                              << "the digital input is off.";
+}
+/**
+ * Tests to see if the DIO PWM functionality works.
+ */
+TEST_F(DIOLoopTest, DIOPWM) {
+  Reset();
+
+  m_output->Set(false);
+  Wait(kDelayTime);
+  EXPECT_FALSE(m_input->Get()) << "The digital output was turned off, but "
+                               << "the digital input is on.";
+
+  // Set frequency to 2.0 Hz
+  m_output->SetPWMRate(2.0);
+  // Enable PWM, but leave it off
+  m_output->EnablePWM(0.0);
+  Wait(0.5);
+  m_output->UpdateDutyCycle(0.5);
+  m_input->RequestInterrupts();
+  m_input->SetUpSourceEdge(false, true);
+  InterruptableSensorBase::WaitResult result =
+      m_input->WaitForInterrupt(3.0, true);
+
+  Wait(0.5);
+  bool firstCycle = m_input->Get();
+  Wait(0.5);
+  bool secondCycle = m_input->Get();
+  Wait(0.5);
+  bool thirdCycle = m_input->Get();
+  Wait(0.5);
+  bool forthCycle = m_input->Get();
+  Wait(0.5);
+  bool fifthCycle = m_input->Get();
+  Wait(0.5);
+  bool sixthCycle = m_input->Get();
+  Wait(0.5);
+  bool seventhCycle = m_input->Get();
+  m_output->DisablePWM();
+  Wait(0.5);
+  bool firstAfterStop = m_input->Get();
+  Wait(0.5);
+  bool secondAfterStop = m_input->Get();
+
+  EXPECT_EQ(InterruptableSensorBase::WaitResult::kFallingEdge, result)
+      << "WaitForInterrupt was not falling.";
+
+  EXPECT_FALSE(firstCycle) << "Input not low after first delay";
+  EXPECT_TRUE(secondCycle) << "Input not high after second delay";
+  EXPECT_FALSE(thirdCycle) << "Input not low after third delay";
+  EXPECT_TRUE(forthCycle) << "Input not high after forth delay";
+  EXPECT_FALSE(fifthCycle) << "Input not low after fifth delay";
+  EXPECT_TRUE(sixthCycle) << "Input not high after sixth delay";
+  EXPECT_FALSE(seventhCycle) << "Input not low after seventh delay";
+  EXPECT_FALSE(firstAfterStop) << "Input not low after stopping first read";
+  EXPECT_FALSE(secondAfterStop) << "Input not low after stopping second read";
+}
+
+/**
+ * Test a fake "counter" that uses the DIO loop as an input to make sure the
+ * Counter class works
+ */
+TEST_F(DIOLoopTest, FakeCounter) {
+  Reset();
+
+  Counter counter(m_input);
+
+  EXPECT_EQ(0, counter.Get()) << "Counter did not initialize to 0.";
+
+  /* Count 100 ticks.  The counter value should be 100 after this loop. */
+  for (int32_t i = 0; i < 100; i++) {
+    m_output->Set(true);
+    Wait(kCounterTime);
+    m_output->Set(false);
+    Wait(kCounterTime);
+  }
+
+  EXPECT_EQ(100, counter.Get()) << "Counter did not count up to 100.";
+}
+
+static void InterruptHandler(uint32_t interruptAssertedMask, void* param) {
+  *reinterpret_cast<int32_t*>(param) = 12345;
+}
+
+TEST_F(DIOLoopTest, AsynchronousInterruptWorks) {
+  int32_t param = 0;
+
+  // Given an interrupt handler that sets an int32_t to 12345
+  m_input->RequestInterrupts(InterruptHandler, &param);
+  m_input->EnableInterrupts();
+
+  // If the voltage rises
+  m_output->Set(false);
+  m_output->Set(true);
+  m_input->CancelInterrupts();
+
+  // Then the int32_t should be 12345
+  Wait(kDelayTime);
+  EXPECT_EQ(12345, param) << "The interrupt did not run.";
+}
+
+static void* InterruptTriggerer(void* data) {
+  DigitalOutput* output = static_cast<DigitalOutput*>(data);
+  output->Set(false);
+  Wait(kSynchronousInterruptTime);
+  output->Set(true);
+  return nullptr;
+}
+
+TEST_F(DIOLoopTest, SynchronousInterruptWorks) {
+  // Given a synchronous interrupt
+  m_input->RequestInterrupts();
+
+  // If we have another thread trigger the interrupt in a few seconds
+  pthread_t interruptTriggererLoop;
+  pthread_create(&interruptTriggererLoop, nullptr, InterruptTriggerer,
+                 m_output);
+
+  // Then this thread should pause and resume after that number of seconds
+  Timer timer;
+  timer.Start();
+  m_input->WaitForInterrupt(kSynchronousInterruptTime + 1.0);
+  EXPECT_NEAR(kSynchronousInterruptTime, timer.Get(),
+              kSynchronousInterruptTimeTolerance);
+}

wpilibcIntegrationTests/src/main/native/cpp/DigitalGlitchFilterTest.cpp

@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "DigitalGlitchFilter.h"  // NOLINT(build/include_order)
+
+#include "Counter.h"
+#include "DigitalInput.h"
+#include "Encoder.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+/**
+ * Tests that configuring inputs to be filtered succeeds.
+ *
+ * This test actually tests everything except that the actual FPGA
+ * implementation works as intended.  We configure the FPGA and then query it to
+ * make sure that the acutal configuration matches.
+ */
+TEST(DigitalGlitchFilterTest, BasicTest) {
+  DigitalInput input1(1);
+  DigitalInput input2(2);
+  DigitalInput input3(3);
+  DigitalInput input4(4);
+  Encoder encoder5(5, 6);
+  Counter counter7(7);
+
+  // Check that we can make a single filter and set the period.
+  DigitalGlitchFilter filter1;
+  filter1.Add(&input1);
+  filter1.SetPeriodNanoSeconds(4200);
+
+  // Check that we can make a second filter with 2 inputs.
+  DigitalGlitchFilter filter2;
+  filter2.Add(&input2);
+  filter2.Add(&input3);
+  filter2.SetPeriodNanoSeconds(97100);
+
+  // Check that we can make a third filter with an input, an encoder, and a
+  // counter.
+  DigitalGlitchFilter filter3;
+  filter3.Add(&input4);
+  filter3.Add(&encoder5);
+  filter3.Add(&counter7);
+  filter3.SetPeriodNanoSeconds(167800);
+
+  // Verify that the period was properly set for all 3 filters.
+  EXPECT_EQ(4200u, filter1.GetPeriodNanoSeconds());
+  EXPECT_EQ(97100u, filter2.GetPeriodNanoSeconds());
+  EXPECT_EQ(167800u, filter3.GetPeriodNanoSeconds());
+
+  // Clean up.
+  filter1.Remove(&input1);
+  filter2.Remove(&input2);
+  filter2.Remove(&input3);
+  filter3.Remove(&input4);
+  filter3.Remove(&encoder5);
+  filter3.Remove(&counter7);
+}

wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp

@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "DriverStation.h"  // NOLINT(build/include_order)
+
+#include "RobotController.h"
+#include "TestBench.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+constexpr double TIMER_TOLERANCE = 0.2;
+constexpr int64_t TIMER_RUNTIME = 1000000;  // 1 second
+
+class DriverStationTest : public testing::Test {};
+
+/**
+ * Test if the WaitForData function works
+ */
+TEST_F(DriverStationTest, WaitForData) {
+  uint64_t initialTime = RobotController::GetFPGATime();
+
+  for (int i = 0; i < 50; i++) {
+    DriverStation::GetInstance().WaitForData();
+  }
+
+  uint64_t finalTime = RobotController::GetFPGATime();
+
+  EXPECT_NEAR(TIMER_RUNTIME, finalTime - initialTime,
+              TIMER_TOLERANCE * TIMER_RUNTIME);
+}

wpilibcIntegrationTests/src/main/native/cpp/FakeEncoderTest.cpp

@@ -0,0 +1,161 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Encoder.h"  // NOLINT(build/include_order)
+
+#include "AnalogOutput.h"
+#include "AnalogTrigger.h"
+#include "DigitalOutput.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const double kDelayTime = 0.001;
+
+class FakeEncoderTest : public testing::Test {
+ protected:
+  DigitalOutput* m_outputA;
+  DigitalOutput* m_outputB;
+  AnalogOutput* m_indexOutput;
+
+  Encoder* m_encoder;
+  AnalogTrigger* m_indexAnalogTrigger;
+  std::shared_ptr<AnalogTriggerOutput> m_indexAnalogTriggerOutput;
+
+  void SetUp() override {
+    m_outputA = new DigitalOutput(TestBench::kLoop2OutputChannel);
+    m_outputB = new DigitalOutput(TestBench::kLoop1OutputChannel);
+    m_indexOutput = new AnalogOutput(TestBench::kAnalogOutputChannel);
+    m_outputA->Set(false);
+    m_outputB->Set(false);
+    m_encoder = new Encoder(TestBench::kLoop1InputChannel,
+                            TestBench::kLoop2InputChannel);
+    m_indexAnalogTrigger =
+        new AnalogTrigger(TestBench::kFakeAnalogOutputChannel);
+    m_indexAnalogTrigger->SetLimitsVoltage(2.0, 3.0);
+    m_indexAnalogTriggerOutput =
+        m_indexAnalogTrigger->CreateOutput(AnalogTriggerType::kState);
+  }
+
+  void TearDown() override {
+    delete m_outputA;
+    delete m_outputB;
+    delete m_indexOutput;
+    delete m_encoder;
+    delete m_indexAnalogTrigger;
+  }
+
+  /**
+   * Output pulses to the encoder's input channels to simulate a change of 100
+   * ticks
+   */
+  void Simulate100QuadratureTicks() {
+    for (int32_t i = 0; i < 100; i++) {
+      m_outputA->Set(true);
+      Wait(kDelayTime);
+      m_outputB->Set(true);
+      Wait(kDelayTime);
+      m_outputA->Set(false);
+      Wait(kDelayTime);
+      m_outputB->Set(false);
+      Wait(kDelayTime);
+    }
+  }
+
+  void SetIndexHigh() {
+    m_indexOutput->SetVoltage(5.0);
+    Wait(kDelayTime);
+  }
+
+  void SetIndexLow() {
+    m_indexOutput->SetVoltage(0.0);
+    Wait(kDelayTime);
+  }
+};
+
+/**
+ * Test the encoder by reseting it to 0 and reading the value.
+ */
+TEST_F(FakeEncoderTest, TestDefaultState) {
+  EXPECT_FLOAT_EQ(0.0, m_encoder->Get()) << "The encoder did not start at 0.";
+}
+
+/**
+ * Test the encoder by setting the digital outputs and reading the value.
+ */
+TEST_F(FakeEncoderTest, TestCountUp) {
+  m_encoder->Reset();
+  Simulate100QuadratureTicks();
+
+  EXPECT_FLOAT_EQ(100.0, m_encoder->Get()) << "Encoder did not count to 100.";
+}
+
+/**
+ * Test that the encoder can stay reset while the index source is high
+ */
+TEST_F(FakeEncoderTest, TestResetWhileHigh) {
+  m_encoder->SetIndexSource(*m_indexAnalogTriggerOutput,
+                            Encoder::IndexingType::kResetWhileHigh);
+
+  SetIndexLow();
+  Simulate100QuadratureTicks();
+  SetIndexHigh();
+  EXPECT_EQ(0, m_encoder->Get());
+
+  Simulate100QuadratureTicks();
+  EXPECT_EQ(0, m_encoder->Get());
+}
+
+/**
+ * Test that the encoder can reset when the index source goes from low to high
+ */
+TEST_F(FakeEncoderTest, TestResetOnRisingEdge) {
+  m_encoder->SetIndexSource(*m_indexAnalogTriggerOutput,
+                            Encoder::IndexingType::kResetOnRisingEdge);
+
+  SetIndexLow();
+  Simulate100QuadratureTicks();
+  SetIndexHigh();
+  EXPECT_EQ(0, m_encoder->Get());
+
+  Simulate100QuadratureTicks();
+  EXPECT_EQ(100, m_encoder->Get());
+}
+
+/**
+ * Test that the encoder can stay reset while the index source is low
+ */
+TEST_F(FakeEncoderTest, TestResetWhileLow) {
+  m_encoder->SetIndexSource(*m_indexAnalogTriggerOutput,
+                            Encoder::IndexingType::kResetWhileLow);
+
+  SetIndexHigh();
+  Simulate100QuadratureTicks();
+  SetIndexLow();
+  EXPECT_EQ(0, m_encoder->Get());
+
+  Simulate100QuadratureTicks();
+  EXPECT_EQ(0, m_encoder->Get());
+}
+
+/**
+ * Test that the encoder can reset when the index source goes from high to low
+ */
+TEST_F(FakeEncoderTest, TestResetOnFallingEdge) {
+  m_encoder->SetIndexSource(*m_indexAnalogTriggerOutput,
+                            Encoder::IndexingType::kResetOnFallingEdge);
+
+  SetIndexHigh();
+  Simulate100QuadratureTicks();
+  SetIndexLow();
+  EXPECT_EQ(0, m_encoder->Get());
+
+  Simulate100QuadratureTicks();
+  EXPECT_EQ(100, m_encoder->Get());
+}

wpilibcIntegrationTests/src/main/native/cpp/FilterNoiseTest.cpp

@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Filters/LinearDigitalFilter.h"  // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+#include <thread>
+
+#include "Base.h"
+#include "TestBench.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+enum FilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
+
+std::ostream& operator<<(std::ostream& os, const FilterNoiseTestType& type) {
+  switch (type) {
+    case TEST_SINGLE_POLE_IIR:
+      os << "LinearDigitalFilter SinglePoleIIR";
+      break;
+    case TEST_MOVAVG:
+      os << "LinearDigitalFilter MovingAverage";
+      break;
+  }
+
+  return os;
+}
+
+constexpr double kStdDev = 10.0;
+
+/**
+ * Adds Gaussian white noise to a function returning data. The noise will have
+ * the standard deviation provided in the constructor.
+ */
+class NoiseGenerator : public PIDSource {
+ public:
+  NoiseGenerator(double (*dataFunc)(double), double stdDev)
+      : m_distr(0.0, stdDev) {
+    m_dataFunc = dataFunc;
+  }
+
+  void SetPIDSourceType(PIDSourceType pidSource) override {}
+
+  double Get() { return m_dataFunc(m_count) + m_noise; }
+
+  double PIDGet() override {
+    m_noise = m_distr(m_gen);
+    m_count += TestBench::kFilterStep;
+    return m_dataFunc(m_count) + m_noise;
+  }
+
+  void Reset() { m_count = -TestBench::kFilterStep; }
+
+ private:
+  std::function<double(double)> m_dataFunc;
+  double m_noise = 0.0;
+
+  // Make sure first call to PIDGet() uses m_count == 0
+  double m_count = -TestBench::kFilterStep;
+
+  std::random_device m_rd;
+  std::mt19937 m_gen{m_rd()};
+  std::normal_distribution<double> m_distr;
+};
+
+/**
+ * A fixture that includes a noise generator wrapped in a filter
+ */
+class FilterNoiseTest : public testing::TestWithParam<FilterNoiseTestType> {
+ protected:
+  std::unique_ptr<PIDSource> m_filter;
+  std::shared_ptr<NoiseGenerator> m_noise;
+
+  static double GetData(double t) { return 100.0 * std::sin(2.0 * M_PI * t); }
+
+  void SetUp() override {
+    m_noise = std::make_shared<NoiseGenerator>(GetData, kStdDev);
+
+    switch (GetParam()) {
+      case TEST_SINGLE_POLE_IIR: {
+        m_filter = std::make_unique<LinearDigitalFilter>(
+            LinearDigitalFilter::SinglePoleIIR(
+                m_noise, TestBench::kSinglePoleIIRTimeConstant,
+                TestBench::kFilterStep));
+        break;
+      }
+
+      case TEST_MOVAVG: {
+        m_filter = std::make_unique<LinearDigitalFilter>(
+            LinearDigitalFilter::MovingAverage(m_noise,
+                                               TestBench::kMovAvgTaps));
+        break;
+      }
+    }
+  }
+};
+
+/**
+ * Test if the filter reduces the noise produced by a signal generator
+ */
+TEST_P(FilterNoiseTest, NoiseReduce) {
+  double theoryData = 0.0;
+  double noiseGenError = 0.0;
+  double filterError = 0.0;
+
+  m_noise->Reset();
+  for (double t = 0; t < TestBench::kFilterTime; t += TestBench::kFilterStep) {
+    theoryData = GetData(t);
+    filterError += std::abs(m_filter->PIDGet() - theoryData);
+    noiseGenError += std::abs(m_noise->Get() - theoryData);
+  }
+
+  RecordProperty("FilterError", filterError);
+
+  // The filter should have produced values closer to the theory
+  EXPECT_GT(noiseGenError, filterError)
+      << "Filter should have reduced noise accumulation but failed";
+}
+
+INSTANTIATE_TEST_CASE_P(Test, FilterNoiseTest,
+                        testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG),);

wpilibcIntegrationTests/src/main/native/cpp/FilterOutputTest.cpp

@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Filters/LinearDigitalFilter.h"  // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+#include <thread>
+
+#include "Base.h"
+#include "TestBench.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+enum FilterOutputTestType {
+  TEST_SINGLE_POLE_IIR,
+  TEST_HIGH_PASS,
+  TEST_MOVAVG,
+  TEST_PULSE
+};
+
+std::ostream& operator<<(std::ostream& os, const FilterOutputTestType& type) {
+  switch (type) {
+    case TEST_SINGLE_POLE_IIR:
+      os << "LinearDigitalFilter SinglePoleIIR";
+      break;
+    case TEST_HIGH_PASS:
+      os << "LinearDigitalFilter HighPass";
+      break;
+    case TEST_MOVAVG:
+      os << "LinearDigitalFilter MovingAverage";
+      break;
+    case TEST_PULSE:
+      os << "LinearDigitalFilter Pulse";
+      break;
+  }
+
+  return os;
+}
+
+class DataWrapper : public PIDSource {
+ public:
+  explicit DataWrapper(double (*dataFunc)(double)) { m_dataFunc = dataFunc; }
+
+  virtual void SetPIDSourceType(PIDSourceType pidSource) {}
+
+  virtual double PIDGet() {
+    m_count += TestBench::kFilterStep;
+    return m_dataFunc(m_count);
+  }
+
+  void Reset() { m_count = -TestBench::kFilterStep; }
+
+ private:
+  std::function<double(double)> m_dataFunc;
+
+  // Make sure first call to PIDGet() uses m_count == 0
+  double m_count = -TestBench::kFilterStep;
+};
+
+/**
+ * A fixture that includes a consistent data source wrapped in a filter
+ */
+class FilterOutputTest : public testing::TestWithParam<FilterOutputTestType> {
+ protected:
+  std::unique_ptr<PIDSource> m_filter;
+  std::shared_ptr<DataWrapper> m_data;
+  double m_expectedOutput = 0.0;
+
+  static double GetData(double t) {
+    return 100.0 * std::sin(2.0 * M_PI * t) + 20.0 * std::cos(50.0 * M_PI * t);
+  }
+
+  static double GetPulseData(double t) {
+    if (std::abs(t - 1.0) < 0.001) {
+      return 1.0;
+    } else {
+      return 0.0;
+    }
+  }
+
+  void SetUp() override {
+    switch (GetParam()) {
+      case TEST_SINGLE_POLE_IIR: {
+        m_data = std::make_shared<DataWrapper>(GetData);
+        m_filter = std::make_unique<LinearDigitalFilter>(
+            LinearDigitalFilter::SinglePoleIIR(
+                m_data, TestBench::kSinglePoleIIRTimeConstant,
+                TestBench::kFilterStep));
+        m_expectedOutput = TestBench::kSinglePoleIIRExpectedOutput;
+        break;
+      }
+
+      case TEST_HIGH_PASS: {
+        m_data = std::make_shared<DataWrapper>(GetData);
+        m_filter =
+            std::make_unique<LinearDigitalFilter>(LinearDigitalFilter::HighPass(
+                m_data, TestBench::kHighPassTimeConstant,
+                TestBench::kFilterStep));
+        m_expectedOutput = TestBench::kHighPassExpectedOutput;
+        break;
+      }
+
+      case TEST_MOVAVG: {
+        m_data = std::make_shared<DataWrapper>(GetData);
+        m_filter = std::make_unique<LinearDigitalFilter>(
+            LinearDigitalFilter::MovingAverage(m_data, TestBench::kMovAvgTaps));
+        m_expectedOutput = TestBench::kMovAvgExpectedOutput;
+        break;
+      }
+
+      case TEST_PULSE: {
+        m_data = std::make_shared<DataWrapper>(GetPulseData);
+        m_filter = std::make_unique<LinearDigitalFilter>(
+            LinearDigitalFilter::MovingAverage(m_data, TestBench::kMovAvgTaps));
+        m_expectedOutput = 0.0;
+        break;
+      }
+    }
+  }
+};
+
+/**
+ * Test if the linear digital filters produce consistent output
+ */
+TEST_P(FilterOutputTest, FilterOutput) {
+  m_data->Reset();
+
+  double filterOutput = 0.0;
+  for (double t = 0.0; t < TestBench::kFilterTime;
+       t += TestBench::kFilterStep) {
+    filterOutput = m_filter->PIDGet();
+  }
+
+  RecordProperty("FilterOutput", filterOutput);
+
+  EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
+      << "Filter output didn't match expected value";
+}
+
+INSTANTIATE_TEST_CASE_P(Test, FilterOutputTest,
+                        testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
+                                        TEST_MOVAVG, TEST_PULSE),);

wpilibcIntegrationTests/src/main/native/cpp/MockSpeedController.cpp

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "MockSpeedController.h"
+
+using namespace frc;
+
+void MockSpeedController::Set(double speed) {
+  m_speed = m_isInverted ? -speed : speed;
+}
+
+double MockSpeedController::Get() const { return m_speed; }
+
+void MockSpeedController::SetInverted(bool isInverted) {
+  m_isInverted = isInverted;
+}
+
+bool MockSpeedController::GetInverted() const { return m_isInverted; }
+
+void MockSpeedController::Disable() { m_speed = 0; }
+
+void MockSpeedController::StopMotor() { Disable(); }
+
+void MockSpeedController::PIDWrite(double output) { Set(output); }

wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp

@@ -0,0 +1,193 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Encoder.h"
+#include "Filters/LinearDigitalFilter.h"
+#include "Jaguar.h"
+#include "PIDController.h"
+#include "Talon.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "Victor.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+enum MotorEncoderTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
+
+std::ostream& operator<<(std::ostream& os, MotorEncoderTestType const& type) {
+  switch (type) {
+    case TEST_VICTOR:
+      os << "Victor";
+      break;
+    case TEST_JAGUAR:
+      os << "Jaguar";
+      break;
+    case TEST_TALON:
+      os << "Talon";
+      break;
+  }
+
+  return os;
+}
+
+static constexpr double kMotorTime = 0.5;
+
+/**
+ * A fixture that includes a PWM speed controller and an encoder connected to
+ * the same motor.
+ */
+class MotorEncoderTest : public testing::TestWithParam<MotorEncoderTestType> {
+ protected:
+  SpeedController* m_speedController;
+  Encoder* m_encoder;
+  LinearDigitalFilter* m_filter;
+
+  void SetUp() override {
+    switch (GetParam()) {
+      case TEST_VICTOR:
+        m_speedController = new Victor(TestBench::kVictorChannel);
+        m_encoder = new Encoder(TestBench::kVictorEncoderChannelA,
+                                TestBench::kVictorEncoderChannelB);
+        break;
+
+      case TEST_JAGUAR:
+        m_speedController = new Jaguar(TestBench::kJaguarChannel);
+        m_encoder = new Encoder(TestBench::kJaguarEncoderChannelA,
+                                TestBench::kJaguarEncoderChannelB);
+        break;
+
+      case TEST_TALON:
+        m_speedController = new Talon(TestBench::kTalonChannel);
+        m_encoder = new Encoder(TestBench::kTalonEncoderChannelA,
+                                TestBench::kTalonEncoderChannelB);
+        break;
+    }
+    m_filter = new LinearDigitalFilter(
+        LinearDigitalFilter::MovingAverage(*m_encoder, 50));
+  }
+
+  void TearDown() override {
+    delete m_speedController;
+    delete m_encoder;
+    delete m_filter;
+  }
+
+  void Reset() {
+    m_speedController->Set(0.0);
+    m_encoder->Reset();
+    m_filter->Reset();
+  }
+};
+
+/**
+ * Test if the encoder value increments after the motor drives forward
+ */
+TEST_P(MotorEncoderTest, Increment) {
+  Reset();
+
+  /* Drive the speed controller briefly to move the encoder */
+  m_speedController->Set(0.2f);
+  Wait(kMotorTime);
+  m_speedController->Set(0.0);
+
+  /* The encoder should be positive now */
+  EXPECT_GT(m_encoder->Get(), 0)
+      << "Encoder should have incremented after the motor moved";
+}
+
+/**
+ * Test if the encoder value decrements after the motor drives backwards
+ */
+TEST_P(MotorEncoderTest, Decrement) {
+  Reset();
+
+  /* Drive the speed controller briefly to move the encoder */
+  m_speedController->Set(-0.2);
+  Wait(kMotorTime);
+  m_speedController->Set(0.0);
+
+  /* The encoder should be positive now */
+  EXPECT_LT(m_encoder->Get(), 0.0)
+      << "Encoder should have decremented after the motor moved";
+}
+
+/**
+ * Test if motor speeds are clamped to [-1,1]
+ */
+TEST_P(MotorEncoderTest, ClampSpeed) {
+  Reset();
+
+  m_speedController->Set(2.0);
+  Wait(kMotorTime);
+
+  EXPECT_FLOAT_EQ(1.0, m_speedController->Get());
+
+  m_speedController->Set(-2.0);
+  Wait(kMotorTime);
+
+  EXPECT_FLOAT_EQ(-1.0, m_speedController->Get());
+}
+
+/**
+ * Test if position PID loop works
+ */
+TEST_P(MotorEncoderTest, PositionPIDController) {
+  Reset();
+  double goal = 1000;
+  m_encoder->SetPIDSourceType(PIDSourceType::kDisplacement);
+  PIDController pid(0.001, 0.0005, 0.0, m_encoder, m_speedController);
+  pid.SetAbsoluteTolerance(50.0);
+  pid.SetOutputRange(-0.2, 0.2);
+  pid.SetSetpoint(goal);
+
+  /* 10 seconds should be plenty time to get to the setpoint */
+  pid.Enable();
+  Wait(10.0);
+  pid.Disable();
+
+  RecordProperty("PIDError", pid.GetError());
+
+  EXPECT_TRUE(pid.OnTarget())
+      << "PID loop did not converge within 10 seconds. Goal was: " << goal
+      << " Error was: " << pid.GetError();
+}
+
+/**
+ * Test if velocity PID loop works
+ */
+TEST_P(MotorEncoderTest, VelocityPIDController) {
+  Reset();
+
+  m_encoder->SetPIDSourceType(PIDSourceType::kRate);
+  PIDController pid(1e-5, 0.0, 3e-5, 8e-5, m_filter, m_speedController);
+  pid.SetAbsoluteTolerance(200.0);
+  pid.SetOutputRange(-0.3, 0.3);
+  pid.SetSetpoint(600);
+
+  /* 10 seconds should be plenty time to get to the setpoint */
+  pid.Enable();
+  Wait(10.0);
+  pid.Disable();
+  RecordProperty("PIDError", pid.GetError());
+
+  EXPECT_TRUE(pid.OnTarget())
+      << "PID loop did not converge within 10 seconds. Goal was: " << 600
+      << " Error was: " << pid.GetError();
+}
+
+/**
+ * Test resetting encoders
+ */
+TEST_P(MotorEncoderTest, Reset) {
+  Reset();
+
+  EXPECT_EQ(0, m_encoder->Get()) << "Encoder did not reset to 0";
+}
+
+INSTANTIATE_TEST_CASE_P(Test, MotorEncoderTest,
+                        testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON),);

wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp

@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Encoder.h"
+#include "Jaguar.h"
+#include "Talon.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "Victor.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+enum MotorInvertingTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
+static const double motorSpeed = 0.15;
+static const double delayTime = 0.5;
+std::ostream& operator<<(std::ostream& os, MotorInvertingTestType const& type) {
+  switch (type) {
+    case TEST_VICTOR:
+      os << "Victor";
+      break;
+    case TEST_JAGUAR:
+      os << "Jaguar";
+      break;
+    case TEST_TALON:
+      os << "Talon";
+      break;
+  }
+
+  return os;
+}
+class MotorInvertingTest
+    : public testing::TestWithParam<MotorInvertingTestType> {
+ protected:
+  SpeedController* m_speedController;
+  Encoder* m_encoder;
+
+  void SetUp() override {
+    switch (GetParam()) {
+      case TEST_VICTOR:
+        m_speedController = new Victor(TestBench::kVictorChannel);
+        m_encoder = new Encoder(TestBench::kVictorEncoderChannelA,
+                                TestBench::kVictorEncoderChannelB);
+        break;
+
+      case TEST_JAGUAR:
+        m_speedController = new Jaguar(TestBench::kJaguarChannel);
+        m_encoder = new Encoder(TestBench::kJaguarEncoderChannelA,
+                                TestBench::kJaguarEncoderChannelB);
+        break;
+
+      case TEST_TALON:
+        m_speedController = new Talon(TestBench::kTalonChannel);
+        m_encoder = new Encoder(TestBench::kTalonEncoderChannelA,
+                                TestBench::kTalonEncoderChannelB);
+        break;
+    }
+  }
+
+  void TearDown() override {
+    delete m_speedController;
+    delete m_encoder;
+  }
+
+  void Reset() {
+    m_speedController->SetInverted(false);
+    m_speedController->Set(0.0);
+    m_encoder->Reset();
+  }
+};
+
+TEST_P(MotorInvertingTest, InvertingPositive) {
+  Reset();
+
+  m_speedController->Set(motorSpeed);
+
+  Wait(delayTime);
+
+  bool initDirection = m_encoder->GetDirection();
+  m_speedController->SetInverted(true);
+  m_speedController->Set(motorSpeed);
+
+  Wait(delayTime);
+
+  EXPECT_TRUE(m_encoder->GetDirection() != initDirection)
+      << "Inverting with Positive value does not change direction";
+
+  Reset();
+}
+
+TEST_P(MotorInvertingTest, InvertingNegative) {
+  Reset();
+
+  m_speedController->SetInverted(false);
+  m_speedController->Set(-motorSpeed);
+
+  Wait(delayTime);
+
+  bool initDirection = m_encoder->GetDirection();
+  m_speedController->SetInverted(true);
+  m_speedController->Set(-motorSpeed);
+
+  Wait(delayTime);
+
+  EXPECT_TRUE(m_encoder->GetDirection() != initDirection)
+      << "Inverting with Negative value does not change direction";
+
+  Reset();
+}
+
+TEST_P(MotorInvertingTest, InvertingSwitchingPosToNeg) {
+  Reset();
+
+  m_speedController->SetInverted(false);
+  m_speedController->Set(motorSpeed);
+
+  Wait(delayTime);
+
+  bool initDirection = m_encoder->GetDirection();
+  m_speedController->SetInverted(true);
+  m_speedController->Set(-motorSpeed);
+
+  Wait(delayTime);
+
+  EXPECT_TRUE(m_encoder->GetDirection() == initDirection)
+      << "Inverting with Switching value does change direction";
+
+  Reset();
+}
+
+TEST_P(MotorInvertingTest, InvertingSwitchingNegToPos) {
+  Reset();
+
+  m_speedController->SetInverted(false);
+  m_speedController->Set(-motorSpeed);
+
+  Wait(delayTime);
+
+  bool initDirection = m_encoder->GetDirection();
+  m_speedController->SetInverted(true);
+  m_speedController->Set(motorSpeed);
+
+  Wait(delayTime);
+
+  EXPECT_TRUE(m_encoder->GetDirection() == initDirection)
+      << "Inverting with Switching value does change direction";
+
+  Reset();
+}
+
+INSTANTIATE_TEST_CASE_P(Test, MotorInvertingTest,
+                        testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON),);

wpilibcIntegrationTests/src/main/native/cpp/NotifierTest.cpp

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Notifier.h"  // NOLINT(build/include_order)
+
+#include <llvm/raw_ostream.h>
+
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+unsigned notifierCounter;
+
+void notifierHandler(void*) { notifierCounter++; }
+
+/**
+ * Test if the Wait function works
+ */
+TEST(NotifierTest, DISABLED_TestTimerNotifications) {
+  llvm::outs() << "NotifierTest...\n";
+  notifierCounter = 0;
+  llvm::outs() << "notifier(notifierHandler, nullptr)...\n";
+  Notifier notifier(notifierHandler, nullptr);
+  llvm::outs() << "Start Periodic...\n";
+  notifier.StartPeriodic(1.0);
+
+  llvm::outs() << "Wait...\n";
+  Wait(10.5);
+  llvm::outs() << "...Wait\n";
+
+  EXPECT_EQ(10u, notifierCounter)
+      << "Received " << notifierCounter << " notifications in 10.5 seconds";
+  llvm::outs() << "Received " << notifierCounter
+               << " notifications in 10.5 seconds";
+
+  llvm::outs() << "...NotifierTest\n";
+}

wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp

@@ -0,0 +1,241 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogInput.h"
+#include "Compressor.h"
+#include "DigitalInput.h"
+#include "DigitalOutput.h"
+#include "DoubleSolenoid.h"
+#include "Solenoid.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+/* The PCM switches the compressor up to a couple seconds after the pressure
+        switch changes. */
+static const double kCompressorDelayTime = 3.0;
+
+/* Solenoids should change much more quickly */
+static const double kSolenoidDelayTime = 0.5;
+
+/* The voltage divider on the test bench should bring the compressor output
+        to around these values. */
+static const double kCompressorOnVoltage = 5.00;
+static const double kCompressorOffVoltage = 1.68;
+
+class PCMTest : public testing::Test {
+ protected:
+  Compressor* m_compressor;
+
+  DigitalOutput* m_fakePressureSwitch;
+  AnalogInput* m_fakeCompressor;
+  DoubleSolenoid* m_doubleSolenoid;
+  DigitalInput *m_fakeSolenoid1, *m_fakeSolenoid2;
+
+  void SetUp() override {
+    m_compressor = new Compressor();
+
+    m_fakePressureSwitch =
+        new DigitalOutput(TestBench::kFakePressureSwitchChannel);
+    m_fakeCompressor = new AnalogInput(TestBench::kFakeCompressorChannel);
+    m_fakeSolenoid1 = new DigitalInput(TestBench::kFakeSolenoid1Channel);
+    m_fakeSolenoid2 = new DigitalInput(TestBench::kFakeSolenoid2Channel);
+  }
+
+  void TearDown() override {
+    delete m_compressor;
+    delete m_fakePressureSwitch;
+    delete m_fakeCompressor;
+    delete m_fakeSolenoid1;
+    delete m_fakeSolenoid2;
+  }
+
+  void Reset() {
+    m_compressor->Stop();
+    m_fakePressureSwitch->Set(false);
+  }
+};
+
+/**
+ * Test if the compressor turns on and off when the pressure switch is toggled
+ */
+TEST_F(PCMTest, PressureSwitch) {
+  Reset();
+
+  m_compressor->SetClosedLoopControl(true);
+
+  // Turn on the compressor
+  m_fakePressureSwitch->Set(true);
+  Wait(kCompressorDelayTime);
+  EXPECT_NEAR(kCompressorOnVoltage, m_fakeCompressor->GetVoltage(), 0.5)
+      << "Compressor did not turn on when the pressure switch turned on.";
+
+  // Turn off the compressor
+  m_fakePressureSwitch->Set(false);
+  Wait(kCompressorDelayTime);
+  EXPECT_NEAR(kCompressorOffVoltage, m_fakeCompressor->GetVoltage(), 0.5)
+      << "Compressor did not turn off when the pressure switch turned off.";
+}
+
+/**
+ * Test if the correct solenoids turn on and off when they should
+ */
+TEST_F(PCMTest, Solenoid) {
+  Reset();
+  Solenoid solenoid1(TestBench::kSolenoidChannel1);
+  Solenoid solenoid2(TestBench::kSolenoidChannel2);
+
+  // Turn both solenoids off
+  solenoid1.Set(false);
+  solenoid2.Set(false);
+  Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
+
+  // Turn one solenoid on and one off
+  solenoid1.Set(true);
+  solenoid2.Set(false);
+  Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
+  EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
+
+  // Turn one solenoid on and one off
+  solenoid1.Set(false);
+  solenoid2.Set(true);
+  Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
+
+  // Turn both on
+  solenoid1.Set(true);
+  solenoid2.Set(true);
+  Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
+  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
+  EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
+}
+
+/**
+ * Test if the correct solenoids turn on and off when they should when used
+ * with the DoubleSolenoid class.
+ */
+TEST_F(PCMTest, DoubleSolenoid) {
+  DoubleSolenoid solenoid(TestBench::kSolenoidChannel1,
+                          TestBench::kSolenoidChannel2);
+
+  solenoid.Set(DoubleSolenoid::kOff);
+  Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kOff)
+      << "Solenoid does not read off";
+
+  solenoid.Set(DoubleSolenoid::kForward);
+  Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kForward)
+      << "Solenoid does not read forward";
+
+  solenoid.Set(DoubleSolenoid::kReverse);
+  Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kReverse)
+      << "Solenoid does not read reverse";
+}
+
+TEST_F(PCMTest, OneShot) {
+  Reset();
+  Solenoid solenoid1(TestBench::kSolenoidChannel1);
+  Solenoid solenoid2(TestBench::kSolenoidChannel2);
+
+  // Turn both solenoids off
+  solenoid1.Set(false);
+  solenoid2.Set(false);
+  Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
+
+  // Pulse Solenoid #1 on, and turn Solenoid #2 off
+  solenoid1.SetPulseDuration(2 * kSolenoidDelayTime);
+  solenoid2.Set(false);
+  solenoid1.StartPulse();
+  Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
+  EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
+  Wait(2 * kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
+
+  // Turn Solenoid #1 off, and pulse Solenoid #2 on
+  solenoid1.Set(false);
+  solenoid2.SetPulseDuration(2 * kSolenoidDelayTime);
+  solenoid2.StartPulse();
+  Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
+  Wait(2 * kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
+
+  // Pulse both Solenoids on
+  solenoid1.SetPulseDuration(2 * kSolenoidDelayTime);
+  solenoid2.SetPulseDuration(2 * kSolenoidDelayTime);
+  solenoid1.StartPulse();
+  solenoid2.StartPulse();
+  Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
+  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
+  EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
+  Wait(2 * kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
+
+  // Pulse both Solenoids on with different durations
+  solenoid1.SetPulseDuration(1.5 * kSolenoidDelayTime);
+  solenoid2.SetPulseDuration(2.5 * kSolenoidDelayTime);
+  solenoid1.StartPulse();
+  solenoid2.StartPulse();
+  Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
+  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
+  EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
+  Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
+  Wait(2 * kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
+  EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
+}

wpilibcIntegrationTests/src/main/native/cpp/PIDToleranceTest.cpp

@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "PIDController.h"
+#include "PIDOutput.h"
+#include "PIDSource.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class PIDToleranceTest : public testing::Test {
+ protected:
+  const double setpoint = 50.0;
+  const double range = 200;
+  const double tolerance = 10.0;
+
+  class FakeInput : public PIDSource {
+   public:
+    double val = 0;
+
+    void SetPIDSourceType(PIDSourceType pidSource) {}
+
+    PIDSourceType GetPIDSourceType() { return PIDSourceType::kDisplacement; }
+
+    double PIDGet() { return val; }
+  };
+
+  class FakeOutput : public PIDOutput {
+    void PIDWrite(double output) {}
+  };
+
+  FakeInput inp;
+  FakeOutput out;
+  PIDController* pid;
+
+  void SetUp() override {
+    pid = new PIDController(0.5, 0.0, 0.0, &inp, &out);
+    pid->SetInputRange(-range / 2, range / 2);
+  }
+
+  void TearDown() override { delete pid; }
+
+  void Reset() { inp.val = 0; }
+};
+
+TEST_F(PIDToleranceTest, Absolute) {
+  Reset();
+
+  pid->SetAbsoluteTolerance(tolerance);
+  pid->SetSetpoint(setpoint);
+  pid->Enable();
+
+  EXPECT_FALSE(pid->OnTarget())
+      << "Error was in tolerance when it should not have been. Error was "
+      << pid->GetError();
+
+  inp.val = setpoint + tolerance / 2;
+  Wait(1.0);
+
+  EXPECT_TRUE(pid->OnTarget())
+      << "Error was not in tolerance when it should have been. Error was "
+      << pid->GetError();
+
+  inp.val = setpoint + 10 * tolerance;
+  Wait(1.0);
+
+  EXPECT_FALSE(pid->OnTarget())
+      << "Error was in tolerance when it should not have been. Error was "
+      << pid->GetError();
+}
+
+TEST_F(PIDToleranceTest, Percent) {
+  Reset();
+
+  pid->SetPercentTolerance(tolerance);
+  pid->SetSetpoint(setpoint);
+  pid->Enable();
+
+  EXPECT_FALSE(pid->OnTarget())
+      << "Error was in tolerance when it should not have been. Error was "
+      << pid->GetError();
+
+  inp.val =
+      setpoint + (tolerance) / 200 *
+                     range;  // half of percent tolerance away from setpoint
+  Wait(1.0);
+
+  EXPECT_TRUE(pid->OnTarget())
+      << "Error was not in tolerance when it should have been. Error was "
+      << pid->GetError();
+
+  inp.val =
+      setpoint +
+      (tolerance) / 50 * range;  // double percent tolerance away from setPoint
+
+  Wait(1.0);
+
+  EXPECT_FALSE(pid->OnTarget())
+      << "Error was in tolerance when it should not have been. Error was "
+      << pid->GetError();
+}

wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp

@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "PowerDistributionPanel.h"  // NOLINT(build/include_order)
+
+#include "Jaguar.h"
+#include "Talon.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "Victor.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const double kMotorTime = 0.25;
+
+class PowerDistributionPanelTest : public testing::Test {
+ protected:
+  PowerDistributionPanel* m_pdp;
+  Talon* m_talon;
+  Victor* m_victor;
+  Jaguar* m_jaguar;
+
+  void SetUp() override {
+    m_pdp = new PowerDistributionPanel();
+    m_talon = new Talon(TestBench::kTalonChannel);
+    m_victor = new Victor(TestBench::kVictorChannel);
+    m_jaguar = new Jaguar(TestBench::kJaguarChannel);
+  }
+
+  void TearDown() override {
+    delete m_pdp;
+    delete m_talon;
+    delete m_victor;
+    delete m_jaguar;
+  }
+};
+
+/**
+ * Test if the current changes when the motor is driven using a talon
+ */
+TEST_F(PowerDistributionPanelTest, CheckCurrentTalon) {
+  Wait(kMotorTime);
+
+  /* The Current should be 0 */
+  EXPECT_FLOAT_EQ(0, m_pdp->GetCurrent(TestBench::kTalonPDPChannel))
+      << "The Talon current was non-zero";
+
+  /* Set the motor to full forward */
+  m_talon->Set(1.0);
+  Wait(kMotorTime);
+
+  /* The current should now be positive */
+  ASSERT_GT(m_pdp->GetCurrent(TestBench::kTalonPDPChannel), 0)
+      << "The Talon current was not positive";
+}

wpilibcIntegrationTests/src/main/native/cpp/PreferencesTest.cpp

@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Preferences.h"  // NOLINT(build/include_order)
+
+#include <cstdio>
+#include <fstream>
+
+#include <networktables/NetworkTableInstance.h>
+
+#include "Timer.h"
+#include "gtest/gtest.h"
+#include "ntcore.h"
+
+using namespace frc;
+
+static const char* kFileName = "networktables.ini";
+static const double kSaveTime = 1.2;
+
+/**
+ * If we write a new networktables.ini with some sample values, test that
+ * we get those same values back using the Preference class.
+ */
+TEST(PreferencesTest, ReadPreferencesFromFile) {
+  auto inst = nt::NetworkTableInstance::GetDefault();
+  inst.StopServer();
+  std::remove(kFileName);
+  std::ofstream preferencesFile(kFileName);
+  preferencesFile << "[NetworkTables Storage 3.0]" << std::endl;
+  preferencesFile
+      << "string \"/Preferences/testFileGetString\"=\"Hello, preferences file\""
+      << std::endl;
+  preferencesFile << "double \"/Preferences/testFileGetInt\"=1" << std::endl;
+  preferencesFile << "double \"/Preferences/testFileGetDouble\"=0.5"
+                  << std::endl;
+  preferencesFile << "double \"/Preferences/testFileGetFloat\"=0.25"
+                  << std::endl;
+  preferencesFile << "boolean \"/Preferences/testFileGetBoolean\"=true"
+                  << std::endl;
+  preferencesFile
+      << "double \"/Preferences/testFileGetLong\"=1000000000000000000"
+      << std::endl;
+  preferencesFile.close();
+  inst.StartServer();
+
+  Preferences* preferences = Preferences::GetInstance();
+  EXPECT_EQ("Hello, preferences file",
+            preferences->GetString("testFileGetString"));
+  EXPECT_EQ(1, preferences->GetInt("testFileGetInt"));
+  EXPECT_FLOAT_EQ(0.5, preferences->GetDouble("testFileGetDouble"));
+  EXPECT_FLOAT_EQ(0.25f, preferences->GetFloat("testFileGetFloat"));
+  EXPECT_TRUE(preferences->GetBoolean("testFileGetBoolean"));
+  EXPECT_EQ(1000000000000000000ll, preferences->GetLong("testFileGetLong"));
+}
+
+/**
+ * If we set some values using the Preferences class, test that they show up
+ * in networktables.ini
+ */
+TEST(PreferencesTest, WritePreferencesToFile) {
+  auto inst = nt::NetworkTableInstance::GetDefault();
+  inst.StartServer();
+  Preferences* preferences = Preferences::GetInstance();
+  preferences->Remove("testFileGetString");
+  preferences->Remove("testFileGetInt");
+  preferences->Remove("testFileGetDouble");
+  preferences->Remove("testFileGetFloat");
+  preferences->Remove("testFileGetBoolean");
+  preferences->Remove("testFileGetLong");
+
+  Wait(kSaveTime);
+
+  preferences->PutString("testFilePutString", "Hello, preferences file");
+  preferences->PutInt("testFilePutInt", 1);
+  preferences->PutDouble("testFilePutDouble", 0.5);
+  preferences->PutFloat("testFilePutFloat", 0.25f);
+  preferences->PutBoolean("testFilePutBoolean", true);
+  preferences->PutLong("testFilePutLong", 1000000000000000000ll);
+
+  Wait(kSaveTime);
+
+  static char const* kExpectedFileContents[] = {
+      "[NetworkTables Storage 3.0]",
+      "string \"/Preferences/.type\"=\"RobotPreferences\"",
+      "boolean \"/Preferences/testFilePutBoolean\"=true",
+      "double \"/Preferences/testFilePutDouble\"=0.5",
+      "double \"/Preferences/testFilePutFloat\"=0.25",
+      "double \"/Preferences/testFilePutInt\"=1",
+      "double \"/Preferences/testFilePutLong\"=1e+18",
+      "string \"/Preferences/testFilePutString\"=\"Hello, preferences file\""};
+
+  std::ifstream preferencesFile(kFileName);
+  for (auto& kExpectedFileContent : kExpectedFileContents) {
+    ASSERT_FALSE(preferencesFile.eof())
+        << "Preferences file prematurely reached EOF";
+
+    std::string line;
+    std::getline(preferencesFile, line);
+
+    ASSERT_EQ(kExpectedFileContent, line)
+        << "A line in networktables.ini was not correct";
+  }
+}

wpilibcIntegrationTests/src/main/native/cpp/RelayTest.cpp

@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Relay.h"  // NOLINT(build/include_order)
+
+#include "DigitalInput.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const double kDelayTime = 0.01;
+
+class RelayTest : public testing::Test {
+ protected:
+  Relay* m_relay;
+  DigitalInput* m_forward;
+  DigitalInput* m_reverse;
+
+  void SetUp() override {
+    m_relay = new Relay(TestBench::kRelayChannel);
+    m_forward = new DigitalInput(TestBench::kFakeRelayForward);
+    m_reverse = new DigitalInput(TestBench::kFakeRelayReverse);
+  }
+
+  void TearDown() override {
+    delete m_relay;
+    delete m_forward;
+    delete m_reverse;
+  }
+
+  void Reset() { m_relay->Set(Relay::kOff); }
+};
+
+/**
+ * Test the relay by setting it forward, reverse, off, and on.
+ */
+TEST_F(RelayTest, Relay) {
+  Reset();
+
+  // set the relay to forward
+  m_relay->Set(Relay::kForward);
+  Wait(kDelayTime);
+  EXPECT_TRUE(m_forward->Get()) << "Relay did not set forward";
+  EXPECT_FALSE(m_reverse->Get()) << "Relay did not set forward";
+  EXPECT_EQ(m_relay->Get(), Relay::kForward);
+
+  // set the relay to reverse
+  m_relay->Set(Relay::kReverse);
+  Wait(kDelayTime);
+  EXPECT_TRUE(m_reverse->Get()) << "Relay did not set reverse";
+  EXPECT_FALSE(m_forward->Get()) << "Relay did not set reverse";
+  EXPECT_EQ(m_relay->Get(), Relay::kReverse);
+
+  // set the relay to off
+  m_relay->Set(Relay::kOff);
+  Wait(kDelayTime);
+  EXPECT_FALSE(m_forward->Get()) << "Relay did not set off";
+  EXPECT_FALSE(m_reverse->Get()) << "Relay did not set off";
+  EXPECT_EQ(m_relay->Get(), Relay::kOff);
+
+  // set the relay to on
+  m_relay->Set(Relay::kOn);
+  Wait(kDelayTime);
+  EXPECT_TRUE(m_forward->Get()) << "Relay did not set on";
+  EXPECT_TRUE(m_reverse->Get()) << "Relay did not set on";
+  EXPECT_EQ(m_relay->Get(), Relay::kOn);
+
+  // test forward direction
+  delete m_relay;
+  m_relay = new Relay(TestBench::kRelayChannel, Relay::kForwardOnly);
+
+  m_relay->Set(Relay::kOn);
+  Wait(kDelayTime);
+  EXPECT_TRUE(m_forward->Get()) << "Relay did not set forward";
+  EXPECT_FALSE(m_reverse->Get()) << "Relay did not set forward";
+  EXPECT_EQ(m_relay->Get(), Relay::kOn);
+
+  // test reverse direction
+  delete m_relay;
+  m_relay = new Relay(TestBench::kRelayChannel, Relay::kReverseOnly);
+
+  m_relay->Set(Relay::kOn);
+  Wait(kDelayTime);
+  EXPECT_FALSE(m_forward->Get()) << "Relay did not set reverse";
+  EXPECT_TRUE(m_reverse->Get()) << "Relay did not set reverse";
+  EXPECT_EQ(m_relay->Get(), Relay::kOn);
+}

wpilibcIntegrationTests/src/main/native/cpp/RobotDriveTest.cpp

@@ -0,0 +1,191 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Drive/DifferentialDrive.h"
+#include "Drive/MecanumDrive.h"
+#include "MockSpeedController.h"
+#include "RobotDrive.h"
+#include "TestBench.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class RobotDriveTest : public testing::Test {
+ protected:
+  MockSpeedController m_rdFrontLeft;
+  MockSpeedController m_rdRearLeft;
+  MockSpeedController m_rdFrontRight;
+  MockSpeedController m_rdRearRight;
+  MockSpeedController m_frontLeft;
+  MockSpeedController m_rearLeft;
+  MockSpeedController m_frontRight;
+  MockSpeedController m_rearRight;
+  frc::RobotDrive m_robotDrive{m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight,
+                               m_rdRearRight};
+  frc::DifferentialDrive m_differentialDrive{m_frontLeft, m_frontRight};
+  frc::MecanumDrive m_mecanumDrive{m_frontLeft, m_rearLeft, m_frontRight,
+                                   m_rearRight};
+
+  double m_testJoystickValues[9] = {-1.0, -0.9, -0.5, -0.01, 0.0,
+                                    0.01, 0.5,  0.9,  1.0};
+  double m_testGyroValues[19] = {0,    45,   90,   135,  180, 225,  270,
+                                 305,  360,  540,  -45,  -90, -135, -180,
+                                 -225, -270, -305, -360, -540};
+};
+
+TEST_F(RobotDriveTest, TankDrive) {
+  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+  double leftJoystick, rightJoystick;
+  m_differentialDrive.SetDeadband(0.0);
+  m_differentialDrive.SetSafetyEnabled(false);
+  m_mecanumDrive.SetSafetyEnabled(false);
+  m_robotDrive.SetSafetyEnabled(false);
+  for (int i = 0; i < joystickSize; i++) {
+    for (int j = 0; j < joystickSize; j++) {
+      leftJoystick = m_testJoystickValues[i];
+      rightJoystick = m_testJoystickValues[j];
+      m_robotDrive.TankDrive(leftJoystick, rightJoystick, false);
+      m_differentialDrive.TankDrive(leftJoystick, rightJoystick, false);
+      ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
+      ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
+    }
+  }
+}
+
+TEST_F(RobotDriveTest, TankDriveSquared) {
+  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+  double leftJoystick, rightJoystick;
+  m_differentialDrive.SetDeadband(0.0);
+  m_differentialDrive.SetSafetyEnabled(false);
+  m_mecanumDrive.SetSafetyEnabled(false);
+  m_robotDrive.SetSafetyEnabled(false);
+  for (int i = 0; i < joystickSize; i++) {
+    for (int j = 0; j < joystickSize; j++) {
+      leftJoystick = m_testJoystickValues[i];
+      rightJoystick = m_testJoystickValues[j];
+      m_robotDrive.TankDrive(leftJoystick, rightJoystick, true);
+      m_differentialDrive.TankDrive(leftJoystick, rightJoystick, true);
+      ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
+      ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
+    }
+  }
+}
+
+TEST_F(RobotDriveTest, ArcadeDriveSquared) {
+  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+  double moveJoystick, rotateJoystick;
+  m_differentialDrive.SetDeadband(0.0);
+  m_differentialDrive.SetSafetyEnabled(false);
+  m_mecanumDrive.SetSafetyEnabled(false);
+  m_robotDrive.SetSafetyEnabled(false);
+  for (int i = 0; i < joystickSize; i++) {
+    for (int j = 0; j < joystickSize; j++) {
+      moveJoystick = m_testJoystickValues[i];
+      rotateJoystick = m_testJoystickValues[j];
+      m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, true);
+      m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, true);
+      ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
+      ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
+    }
+  }
+}
+
+TEST_F(RobotDriveTest, ArcadeDrive) {
+  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+  double moveJoystick, rotateJoystick;
+  m_differentialDrive.SetDeadband(0.0);
+  m_differentialDrive.SetSafetyEnabled(false);
+  m_mecanumDrive.SetSafetyEnabled(false);
+  m_robotDrive.SetSafetyEnabled(false);
+  for (int i = 0; i < joystickSize; i++) {
+    for (int j = 0; j < joystickSize; j++) {
+      moveJoystick = m_testJoystickValues[i];
+      rotateJoystick = m_testJoystickValues[j];
+      m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, false);
+      m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, false);
+      ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
+      ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
+    }
+  }
+}
+
+TEST_F(RobotDriveTest, MecanumCartesian) {
+  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+  int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
+  double xJoystick, yJoystick, rotateJoystick, gyroValue;
+  m_mecanumDrive.SetDeadband(0.0);
+  m_mecanumDrive.SetSafetyEnabled(false);
+  m_differentialDrive.SetSafetyEnabled(false);
+  m_robotDrive.SetSafetyEnabled(false);
+  for (int i = 0; i < joystickSize; i++) {
+    for (int j = 0; j < joystickSize; j++) {
+      for (int k = 0; k < joystickSize; k++) {
+        for (int l = 0; l < gyroSize; l++) {
+          xJoystick = m_testJoystickValues[i];
+          yJoystick = m_testJoystickValues[j];
+          rotateJoystick = m_testJoystickValues[k];
+          gyroValue = m_testGyroValues[l];
+          m_robotDrive.MecanumDrive_Cartesian(xJoystick, yJoystick,
+                                              rotateJoystick, gyroValue);
+          m_mecanumDrive.DriveCartesian(xJoystick, -yJoystick, rotateJoystick,
+                                        -gyroValue);
+          ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
+              << "X: " << xJoystick << " Y: " << yJoystick
+              << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
+          ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
+              << "X: " << xJoystick << " Y: " << yJoystick
+              << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
+          ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
+              << "X: " << xJoystick << " Y: " << yJoystick
+              << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
+          ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
+              << "X: " << xJoystick << " Y: " << yJoystick
+              << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
+        }
+      }
+    }
+  }
+}
+
+TEST_F(RobotDriveTest, MecanumPolar) {
+  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
+  int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
+  double magnitudeJoystick, directionJoystick, rotateJoystick;
+  m_mecanumDrive.SetDeadband(0.0);
+  m_mecanumDrive.SetSafetyEnabled(false);
+  m_differentialDrive.SetSafetyEnabled(false);
+  m_robotDrive.SetSafetyEnabled(false);
+  for (int i = 0; i < joystickSize; i++) {
+    for (int j = 0; j < gyroSize; j++) {
+      for (int k = 0; k < joystickSize; k++) {
+        magnitudeJoystick = m_testJoystickValues[i];
+        directionJoystick = m_testGyroValues[j];
+        rotateJoystick = m_testJoystickValues[k];
+        m_robotDrive.MecanumDrive_Polar(magnitudeJoystick, directionJoystick,
+                                        rotateJoystick);
+        m_mecanumDrive.DrivePolar(magnitudeJoystick, directionJoystick,
+                                  rotateJoystick);
+        ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
+            << "Magnitude: " << magnitudeJoystick
+            << " Direction: " << directionJoystick
+            << " Rotate: " << rotateJoystick;
+        ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
+            << "Magnitude: " << magnitudeJoystick
+            << " Direction: " << directionJoystick
+            << " Rotate: " << rotateJoystick;
+        ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
+            << "Magnitude: " << magnitudeJoystick
+            << " Direction: " << directionJoystick
+            << " Rotate: " << rotateJoystick;
+        ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
+            << "Magnitude: " << magnitudeJoystick
+            << " Direction: " << directionJoystick
+            << " Rotate: " << rotateJoystick;
+      }
+    }
+  }
+}

wpilibcIntegrationTests/src/main/native/cpp/SpeedControllerGroupTest.cpp

@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "SpeedControllerGroup.h"  // NOLINT(build/include_order)
+
+#include <memory>
+#include <vector>
+
+#include "MockSpeedController.h"
+#include "TestBench.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+enum SpeedControllerGroupTestType { TEST_ONE, TEST_TWO, TEST_THREE };
+
+std::ostream& operator<<(std::ostream& os,
+                         const SpeedControllerGroupTestType& type) {
+  switch (type) {
+    case TEST_ONE:
+      os << "SpeedControllerGroup with one speed controller";
+      break;
+    case TEST_TWO:
+      os << "SpeedControllerGroup with two speed controllers";
+      break;
+    case TEST_THREE:
+      os << "SpeedControllerGroup with three speed controllers";
+      break;
+  }
+
+  return os;
+}
+
+/**
+ * A fixture used for SpeedControllerGroup testing.
+ */
+class SpeedControllerGroupTest
+    : public testing::TestWithParam<SpeedControllerGroupTestType> {
+ protected:
+  std::vector<MockSpeedController> m_speedControllers;
+  std::unique_ptr<SpeedControllerGroup> m_group;
+
+  void SetUp() override {
+    switch (GetParam()) {
+      case TEST_ONE: {
+        m_speedControllers.emplace_back();
+        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0]);
+        break;
+      }
+
+      case TEST_TWO: {
+        m_speedControllers.emplace_back();
+        m_speedControllers.emplace_back();
+        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0],
+                                                         m_speedControllers[1]);
+        break;
+      }
+
+      case TEST_THREE: {
+        m_speedControllers.emplace_back();
+        m_speedControllers.emplace_back();
+        m_speedControllers.emplace_back();
+        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0],
+                                                         m_speedControllers[1],
+                                                         m_speedControllers[2]);
+        break;
+      }
+    }
+  }
+};
+
+TEST_P(SpeedControllerGroupTest, Set) {
+  m_group->Set(1.0);
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), 1.0);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, GetInverted) {
+  m_group->SetInverted(true);
+
+  EXPECT_TRUE(m_group->GetInverted());
+}
+
+TEST_P(SpeedControllerGroupTest, SetInvertedDoesNotModifySpeedControllers) {
+  for (auto& speedController : m_speedControllers) {
+    speedController.SetInverted(false);
+  }
+  m_group->SetInverted(true);
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_EQ(speedController.GetInverted(), false);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, SetInvertedDoesInvert) {
+  m_group->SetInverted(true);
+  m_group->Set(1.0);
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), -1.0);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, Disable) {
+  m_group->Set(1.0);
+  m_group->Disable();
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), 0.0);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, StopMotor) {
+  m_group->Set(1.0);
+  m_group->StopMotor();
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), 0.0);
+  }
+}
+
+TEST_P(SpeedControllerGroupTest, PIDWrite) {
+  m_group->PIDWrite(1.0);
+
+  for (auto& speedController : m_speedControllers) {
+    EXPECT_FLOAT_EQ(speedController.Get(), 1.0);
+  }
+}
+
+INSTANTIATE_TEST_CASE_P(Test, SpeedControllerGroupTest,
+                        testing::Values(TEST_ONE, TEST_TWO, TEST_THREE), );

wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cstdlib>
+
+#include <HAL/HAL.h>
+#include <llvm/raw_ostream.h>
+
+#include "DriverStation.h"
+#include "LiveWindow/LiveWindow.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+#include "mockds/MockDS.h"
+
+using namespace frc;
+
+class TestEnvironment : public testing::Environment {
+  bool m_alreadySetUp = false;
+  MockDS m_mockDS;
+
+ public:
+  void SetUp() override {
+    /* Only set up once.  This allows gtest_repeat to be used to
+            automatically repeat tests. */
+    if (m_alreadySetUp) return;
+    m_alreadySetUp = true;
+
+    if (!HAL_Initialize(500, 0)) {
+      llvm::errs() << "FATAL ERROR: HAL could not be initialized\n";
+      std::exit(-1);
+    }
+
+    m_mockDS.start();
+
+    /* This sets up the network communications library to enable the driver
+            station. After starting network coms, it will loop until the driver
+            station returns that the robot is enabled, to ensure that tests
+            will be able to run on the hardware. */
+    HAL_ObserveUserProgramStarting();
+    LiveWindow::GetInstance()->SetEnabled(false);
+
+    llvm::outs() << "Started coms\n";
+
+    int enableCounter = 0;
+    while (!DriverStation::GetInstance().IsEnabled()) {
+      if (enableCounter > 50) {
+        // Robot did not enable properly after 5 seconds.
+        // Force exit
+        llvm::errs() << " Failed to enable. Aborting\n";
+        std::terminate();
+      }
+
+      Wait(0.1);
+
+      llvm::outs() << "Waiting for enable: " << enableCounter++ << "\n";
+    }
+  }
+
+  void TearDown() override { m_mockDS.stop(); }
+};
+
+testing::Environment* const environment =
+    testing::AddGlobalTestEnvironment(new TestEnvironment);

wpilibcIntegrationTests/src/main/native/cpp/TiltPanCameraTest.cpp

@@ -0,0 +1,172 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "ADXL345_SPI.h"
+#include "AnalogGyro.h"
+#include "Servo.h"
+#include "TestBench.h"
+#include "Timer.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kServoResetTime = 2.0;
+
+static constexpr double kTestAngle = 90.0;
+
+static constexpr double kTiltSetpoint0 = 0.22;
+static constexpr double kTiltSetpoint45 = 0.45;
+static constexpr double kTiltSetpoint90 = 0.68;
+static constexpr double kTiltTime = 1.0;
+static constexpr double kAccelerometerTolerance = 0.2;
+static constexpr double kSensitivity = 0.013;
+
+/**
+ * A fixture for the camera with two servos and a gyro
+ */
+class TiltPanCameraTest : public testing::Test {
+ protected:
+  static AnalogGyro* m_gyro;
+  Servo *m_tilt, *m_pan;
+  Accelerometer* m_spiAccel;
+
+  static void SetUpTestCase() {
+    // The gyro object blocks for 5 seconds in the constructor, so only
+    // construct it once for the whole test case
+    m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel);
+    m_gyro->SetSensitivity(kSensitivity);
+  }
+
+  static void TearDownTestCase() { delete m_gyro; }
+
+  void SetUp() override {
+    m_tilt = new Servo(TestBench::kCameraTiltChannel);
+    m_pan = new Servo(TestBench::kCameraPanChannel);
+    m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS1);
+
+    m_tilt->Set(kTiltSetpoint45);
+    m_pan->SetAngle(0.0);
+
+    Wait(kServoResetTime);
+
+    m_gyro->Reset();
+  }
+
+  void DefaultGyroAngle();
+  void GyroAngle();
+  void GyroCalibratedParameters();
+
+  void TearDown() override {
+    delete m_tilt;
+    delete m_pan;
+    delete m_spiAccel;
+  }
+};
+
+AnalogGyro* TiltPanCameraTest::m_gyro = nullptr;
+
+/**
+ * Test if the gyro angle defaults to 0 immediately after being reset.
+ */
+void TiltPanCameraTest::DefaultGyroAngle() {
+  EXPECT_NEAR(0.0, m_gyro->GetAngle(), 1.0);
+}
+
+/**
+ * Test if the servo turns 90 degrees and the gyroscope measures this angle
+ * Note servo on TestBench is not the same type of servo that servo class
+ * was designed for so setAngle is significantly off. This has been calibrated
+ * for the servo on the rig.
+ */
+void TiltPanCameraTest::GyroAngle() {
+  // Make sure that the gyro doesn't get jerked when the servo goes to zero.
+  m_pan->SetAngle(0.0);
+  Wait(0.5);
+  m_gyro->Reset();
+
+  for (int32_t i = 0; i < 600; i++) {
+    m_pan->Set(i / 1000.0);
+    Wait(0.001);
+  }
+
+  double gyroAngle = m_gyro->GetAngle();
+
+  EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
+      << "Gyro measured " << gyroAngle << " degrees, servo should have turned "
+      << kTestAngle << " degrees";
+}
+
+/**
+ * Gets calibrated parameters from previously calibrated gyro, allocates a new
+ * gyro with the given parameters for center and offset, and re-runs tests on
+ * the new gyro.
+ */
+void TiltPanCameraTest::GyroCalibratedParameters() {
+  uint32_t cCenter = m_gyro->GetCenter();
+  double cOffset = m_gyro->GetOffset();
+  delete m_gyro;
+  m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel, cCenter, cOffset);
+  m_gyro->SetSensitivity(kSensitivity);
+
+  // Default gyro angle test
+  // Accumulator needs a small amount of time to reset before being tested
+  m_gyro->Reset();
+  Wait(0.001);
+  EXPECT_NEAR(0.0, m_gyro->GetAngle(), 1.0);
+
+  // Gyro angle test
+  // Make sure that the gyro doesn't get jerked when the servo goes to zero.
+  m_pan->SetAngle(0.0);
+  Wait(0.5);
+  m_gyro->Reset();
+
+  for (int32_t i = 0; i < 600; i++) {
+    m_pan->Set(i / 1000.0);
+    Wait(0.001);
+  }
+
+  double gyroAngle = m_gyro->GetAngle();
+
+  EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
+      << "Gyro measured " << gyroAngle << " degrees, servo should have turned "
+      << kTestAngle << " degrees";
+}
+
+/**
+ * Run all gyro tests in one function to make sure they are run in order.
+ */
+TEST_F(TiltPanCameraTest, TestAllGyroTests) {
+  DefaultGyroAngle();
+  GyroAngle();
+  GyroCalibratedParameters();
+}
+
+/**
+ * Test if the accelerometer measures gravity along the correct axes when the
+ * camera rotates
+ */
+TEST_F(TiltPanCameraTest, SPIAccelerometer) {
+  m_tilt->Set(kTiltSetpoint0);
+  Wait(kTiltTime);
+  EXPECT_NEAR(-1.0, m_spiAccel->GetX(), kAccelerometerTolerance);
+  EXPECT_NEAR(0.0, m_spiAccel->GetY(), kAccelerometerTolerance);
+  EXPECT_NEAR(0.0, m_spiAccel->GetZ(), kAccelerometerTolerance);
+
+  m_tilt->Set(kTiltSetpoint45);
+  Wait(kTiltTime);
+  EXPECT_NEAR(-std::sqrt(0.5), m_spiAccel->GetX(), kAccelerometerTolerance);
+  EXPECT_NEAR(0.0, m_spiAccel->GetY(), kAccelerometerTolerance);
+  EXPECT_NEAR(std::sqrt(0.5), m_spiAccel->GetZ(), kAccelerometerTolerance);
+
+  m_tilt->Set(kTiltSetpoint90);
+  Wait(kTiltTime);
+  EXPECT_NEAR(0.0, m_spiAccel->GetX(), kAccelerometerTolerance);
+  EXPECT_NEAR(0.0, m_spiAccel->GetY(), kAccelerometerTolerance);
+  EXPECT_NEAR(1.0, m_spiAccel->GetZ(), kAccelerometerTolerance);
+}

wpilibcIntegrationTests/src/main/native/cpp/TimerTest.cpp

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Timer.h"  // NOLINT(build/include_order)
+
+#include "TestBench.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static const double kWaitTime = 0.5;
+
+class TimerTest : public testing::Test {
+ protected:
+  Timer* m_timer;
+
+  void SetUp() override { m_timer = new Timer; }
+
+  void TearDown() override { delete m_timer; }
+
+  void Reset() { m_timer->Reset(); }
+};
+
+/**
+ * Test if the Wait function works
+ */
+TEST_F(TimerTest, Wait) {
+  Reset();
+
+  double initialTime = m_timer->GetFPGATimestamp();
+
+  Wait(kWaitTime);
+
+  double finalTime = m_timer->GetFPGATimestamp();
+
+  EXPECT_NEAR(kWaitTime, finalTime - initialTime, 0.001);
+}

wpilibcIntegrationTests/src/main/native/cpp/command/CommandTest.cpp

@@ -0,0 +1,444 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Commands/CommandGroup.h"
+#include "Commands/Scheduler.h"
+#include "Commands/Subsystem.h"
+#include "DriverStation.h"
+#include "RobotState.h"
+#include "Timer.h"
+#include "command/MockCommand.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class CommandTest : public testing::Test {
+ protected:
+  void SetUp() override {
+    RobotState::SetImplementation(DriverStation::GetInstance());
+    Scheduler::GetInstance()->SetEnabled(true);
+  }
+
+  /**
+   * Tears Down the Scheduler at the end of each test.
+   * Must be called at the end of each test inside each test in order to prevent
+   * them being deallocated
+   * when they leave the scope of the test (causing a segfault).
+   * This can not be done within the virtual void Teardown() method because it
+   * is called outside of the
+   * scope of the test.
+   */
+  void TeardownScheduler() { Scheduler::GetInstance()->ResetAll(); }
+
+  void AssertCommandState(MockCommand& command, int32_t initialize,
+                          int32_t execute, int32_t isFinished, int32_t end,
+                          int32_t interrupted) {
+    EXPECT_EQ(initialize, command.GetInitializeCount());
+    EXPECT_EQ(execute, command.GetExecuteCount());
+    EXPECT_EQ(isFinished, command.GetIsFinishedCount());
+    EXPECT_EQ(end, command.GetEndCount());
+    EXPECT_EQ(interrupted, command.GetInterruptedCount());
+  }
+};
+
+class ASubsystem : public Subsystem {
+ private:
+  Command* m_command = nullptr;
+
+ public:
+  explicit ASubsystem(const std::string& name) : Subsystem(name) {}
+
+  void InitDefaultCommand() override {
+    if (m_command != nullptr) {
+      SetDefaultCommand(m_command);
+    }
+  }
+
+  void Init(Command* command) { m_command = command; }
+};
+
+// CommandParallelGroupTest ported from CommandParallelGroupTest.java
+TEST_F(CommandTest, ParallelCommands) {
+  MockCommand command1;
+  MockCommand command2;
+  CommandGroup commandGroup;
+
+  commandGroup.AddParallel(&command1);
+  commandGroup.AddParallel(&command2);
+
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+  commandGroup.Start();
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 0);
+  AssertCommandState(command2, 1, 1, 1, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 2, 2, 0, 0);
+  AssertCommandState(command2, 1, 2, 2, 0, 0);
+  command1.SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 3, 3, 1, 0);
+  AssertCommandState(command2, 1, 3, 3, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 3, 3, 1, 0);
+  AssertCommandState(command2, 1, 4, 4, 0, 0);
+  command2.SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 3, 3, 1, 0);
+  AssertCommandState(command2, 1, 5, 5, 1, 0);
+
+  TeardownScheduler();
+}
+// END CommandParallelGroupTest
+
+// CommandScheduleTest ported from CommandScheduleTest.java
+TEST_F(CommandTest, RunAndTerminate) {
+  MockCommand command;
+  command.Start();
+  AssertCommandState(command, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 1, 1, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 2, 2, 0, 0);
+  command.SetHasFinished(true);
+  AssertCommandState(command, 1, 2, 2, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 3, 3, 1, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 3, 3, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(CommandTest, RunAndCancel) {
+  MockCommand command;
+  command.Start();
+  AssertCommandState(command, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 1, 1, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 2, 2, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 3, 3, 0, 0);
+  command.Cancel();
+  AssertCommandState(command, 1, 3, 3, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 3, 3, 0, 1);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 3, 3, 0, 1);
+
+  TeardownScheduler();
+}
+// END CommandScheduleTest
+
+// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
+TEST_F(CommandTest, ThreeCommandOnSubSystem) {
+  ASubsystem subsystem("Three Command Test Subsystem");
+  MockCommand command1;
+  command1.Requires(&subsystem);
+  MockCommand command2;
+  command2.Requires(&subsystem);
+  MockCommand command3;
+  command3.Requires(&subsystem);
+
+  CommandGroup commandGroup;
+  commandGroup.AddSequential(&command1, 1.0);
+  commandGroup.AddSequential(&command2, 2.0);
+  commandGroup.AddSequential(&command3);
+
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+  AssertCommandState(command3, 0, 0, 0, 0, 0);
+
+  commandGroup.Start();
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+  AssertCommandState(command3, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+  AssertCommandState(command3, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+  AssertCommandState(command3, 0, 0, 0, 0, 0);
+  Wait(1);  // command 1 timeout
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 1);
+  AssertCommandState(command2, 1, 1, 1, 0, 0);
+  AssertCommandState(command3, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 1);
+  AssertCommandState(command2, 1, 2, 2, 0, 0);
+  AssertCommandState(command3, 0, 0, 0, 0, 0);
+  Wait(2);  // command 2 timeout
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 1);
+  AssertCommandState(command2, 1, 2, 2, 0, 1);
+  AssertCommandState(command3, 1, 1, 1, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 1);
+  AssertCommandState(command2, 1, 2, 2, 0, 1);
+  AssertCommandState(command3, 1, 2, 2, 0, 0);
+  command3.SetHasFinished(true);
+  AssertCommandState(command1, 1, 1, 1, 0, 1);
+  AssertCommandState(command2, 1, 2, 2, 0, 1);
+  AssertCommandState(command3, 1, 2, 2, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 1);
+  AssertCommandState(command2, 1, 2, 2, 0, 1);
+  AssertCommandState(command3, 1, 3, 3, 1, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 1);
+  AssertCommandState(command2, 1, 2, 2, 0, 1);
+  AssertCommandState(command3, 1, 3, 3, 1, 0);
+
+  TeardownScheduler();
+}
+// END CommandSequentialGroupTest
+
+// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
+TEST_F(CommandTest, OneCommandSupersedingAnotherBecauseOfDependencies) {
+  auto subsystem = new ASubsystem("Command Superseding Test Subsystem");
+  MockCommand command1;
+  command1.Requires(subsystem);
+  MockCommand command2;
+  command2.Requires(subsystem);
+
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  command1.Start();
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 2, 2, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 3, 3, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  command2.Start();
+  AssertCommandState(command1, 1, 3, 3, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 4, 4, 0, 1);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 4, 4, 0, 1);
+  AssertCommandState(command2, 1, 1, 1, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 4, 4, 0, 1);
+  AssertCommandState(command2, 1, 2, 2, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 4, 4, 0, 1);
+  AssertCommandState(command2, 1, 3, 3, 0, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(CommandTest,
+       OneCommandFailingSupersedingBecauseFirstCanNotBeInterrupted) {
+  ASubsystem subsystem("Command Superseding Test Subsystem");
+  MockCommand command1;
+
+  command1.Requires(&subsystem);
+
+  command1.SetInterruptible(false);
+  MockCommand command2;
+  command2.Requires(&subsystem);
+
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  command1.Start();
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 0, 0, 0, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 1, 1, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 2, 2, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 3, 3, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  command2.Start();
+  AssertCommandState(command1, 1, 3, 3, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command1, 1, 4, 4, 0, 0);
+  AssertCommandState(command2, 0, 0, 0, 0, 0);
+
+  TeardownScheduler();
+}
+
+// END CommandSequentialGroupTest
+
+class ModifiedMockCommand : public MockCommand {
+ public:
+  ModifiedMockCommand() : MockCommand() { SetTimeout(2.0); }
+  bool IsFinished() override {
+    return MockCommand::IsFinished() || IsTimedOut();
+  }
+};
+
+TEST_F(CommandTest, TwoSecondTimeout) {
+  ASubsystem subsystem("Two Second Timeout Test Subsystem");
+  ModifiedMockCommand command;
+  command.Requires(&subsystem);
+
+  command.Start();
+  AssertCommandState(command, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 1, 1, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 2, 2, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 3, 3, 0, 0);
+  Wait(2);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 4, 4, 1, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(command, 1, 4, 4, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(CommandTest, DefaultCommandWhereTheInteruptingCommandEndsItself) {
+  ASubsystem subsystem("Default Command Test Subsystem");
+  MockCommand defaultCommand;
+  defaultCommand.Requires(&subsystem);
+  MockCommand anotherCommand;
+  anotherCommand.Requires(&subsystem);
+
+  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+  subsystem.Init(&defaultCommand);
+
+  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 1, 1, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+
+  anotherCommand.Start();
+  AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+  AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 1, 1, 1, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+  anotherCommand.SetHasFinished(true);
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 2, 4, 4, 0, 1);
+  AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 2, 5, 5, 0, 1);
+  AssertCommandState(anotherCommand, 1, 3, 3, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(CommandTest, DefaultCommandsInterruptingCommandCanceled) {
+  ASubsystem subsystem("Default Command Test Subsystem");
+  MockCommand defaultCommand;
+  defaultCommand.Requires(&subsystem);
+  MockCommand anotherCommand;
+  anotherCommand.Requires(&subsystem);
+
+  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+  subsystem.Init(&defaultCommand);
+  subsystem.InitDefaultCommand();
+  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 1, 1, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+
+  anotherCommand.Start();
+  AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
+  AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 0, 0, 0, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 1, 1, 1, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+  anotherCommand.Cancel();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 1, 2, 2, 0, 0);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
+  AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 2, 4, 4, 0, 1);
+  AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(defaultCommand, 2, 5, 5, 0, 1);
+  AssertCommandState(anotherCommand, 1, 2, 2, 0, 1);
+
+  TeardownScheduler();
+}

wpilibcIntegrationTests/src/main/native/cpp/command/ConditionalCommandTest.cpp

@@ -0,0 +1,437 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <DriverStation.h>
+#include <RobotState.h>
+
+#include "Commands/ConditionalCommand.h"
+#include "Commands/Scheduler.h"
+#include "Commands/Subsystem.h"
+#include "command/MockCommand.h"
+#include "command/MockConditionalCommand.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+class ConditionalCommandTest : public testing::Test {
+ public:
+  MockConditionalCommand* m_command;
+  MockCommand* m_onTrue;
+  MockCommand* m_onFalse;
+  MockConditionalCommand* m_commandNull;
+  Subsystem* m_subsystem;
+
+ protected:
+  void SetUp() override {
+    RobotState::SetImplementation(DriverStation::GetInstance());
+    Scheduler::GetInstance()->SetEnabled(true);
+
+    m_subsystem = new Subsystem("MockSubsystem");
+    m_onTrue = new MockCommand(m_subsystem);
+    m_onFalse = new MockCommand(m_subsystem);
+    m_command = new MockConditionalCommand(m_onTrue, m_onFalse);
+    m_commandNull = new MockConditionalCommand(m_onTrue, nullptr);
+  }
+
+  void TearDown() override { delete m_command; }
+
+  /**
+   * Tears Down the Scheduler at the end of each test.
+   *
+   * Must be called at the end of each test inside each test in order to prevent
+   * them being deallocated when they leave the scope of the test (causing a
+   * segfault). This cannot be done within the virtual void Teardown() method
+   * because it is called outside of the scope of the test.
+   */
+  void TeardownScheduler() { Scheduler::GetInstance()->ResetAll(); }
+
+  void AssertCommandState(MockCommand& command, int32_t initialize,
+                          int32_t execute, int32_t isFinished, int32_t end,
+                          int32_t interrupted) {
+    EXPECT_EQ(initialize, command.GetInitializeCount());
+    EXPECT_EQ(execute, command.GetExecuteCount());
+    EXPECT_EQ(isFinished, command.GetIsFinishedCount());
+    EXPECT_EQ(end, command.GetEndCount());
+    EXPECT_EQ(interrupted, command.GetInterruptedCount());
+  }
+
+  void AssertConditionalCommandState(MockConditionalCommand& command,
+                                     int32_t initialize, int32_t execute,
+                                     int32_t isFinished, int32_t end,
+                                     int32_t interrupted) {
+    EXPECT_EQ(initialize, command.GetInitializeCount());
+    EXPECT_EQ(execute, command.GetExecuteCount());
+    EXPECT_EQ(isFinished, command.GetIsFinishedCount());
+    EXPECT_EQ(end, command.GetEndCount());
+    EXPECT_EQ(interrupted, command.GetInterruptedCount());
+  }
+};
+
+TEST_F(ConditionalCommandTest, OnTrueTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  EXPECT_TRUE(m_onTrue->GetInitializeCount() > 0)
+      << "Did not initialize the true command\n";
+  EXPECT_TRUE(m_onFalse->GetInitializeCount() == 0)
+      << "Initialized the false command\n";
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnFalseTest) {
+  m_command->SetCondition(false);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onFalse, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onFalse, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onFalse->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onFalse, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onFalse, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  EXPECT_TRUE(m_onFalse->GetInitializeCount() > 0)
+      << "Did not initialize the false command";
+  EXPECT_TRUE(m_onTrue->GetInitializeCount() == 0)
+      << "Initialized the true command";
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, CancelSubCommandTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->Cancel();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, CancelCondCommandTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_command->Cancel();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 1);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 1);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnTrueTwiceTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  m_onTrue->ResetCounters();
+  m_command->ResetCounters();
+  Scheduler::GetInstance()->AddCommand(m_command);
+
+  SCOPED_TRACE("11");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("12");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("13");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("14");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("15");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("16");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("17");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnTrueInstantTest) {
+  m_command->SetCondition(true);
+  m_onTrue->SetHasFinished(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 1, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, CancelRequiresTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onFalse->Start();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 0, 1);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 0, 1);
+  AssertCommandState(*m_onFalse, 1, 1, 1, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 0, 1);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnFalseNullTest) {
+  m_command->SetCondition(false);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_commandNull);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 1, 1, 1, 1, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 1, 1, 1, 1, 0);
+
+  TeardownScheduler();
+}

wpilibcIntegrationTests/src/main/native/cpp/command/MockCommand.cpp

@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "command/MockCommand.h"
+
+using namespace frc;
+
+MockCommand::MockCommand(Subsystem* subsys) : MockCommand() {
+  Requires(subsys);
+}
+
+MockCommand::MockCommand() {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_hasFinished = false;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}
+
+bool MockCommand::HasInitialized() { return GetInitializeCount() > 0; }
+
+bool MockCommand::HasEnd() { return GetEndCount() > 0; }
+
+bool MockCommand::HasInterrupted() { return GetInterruptedCount() > 0; }
+
+void MockCommand::Initialize() { ++m_initializeCount; }
+
+void MockCommand::Execute() { ++m_executeCount; }
+
+bool MockCommand::IsFinished() {
+  ++m_isFinishedCount;
+  return IsHasFinished();
+}
+
+void MockCommand::End() { ++m_endCount; }
+
+void MockCommand::Interrupted() { ++m_interruptedCount; }
+
+void MockCommand::ResetCounters() {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_hasFinished = false;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}

wpilibcIntegrationTests/src/main/native/cpp/command/MockConditionalCommand.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "command/MockConditionalCommand.h"
+
+using namespace frc;
+
+MockConditionalCommand::MockConditionalCommand(MockCommand* onTrue,
+                                               MockCommand* onFalse)
+    : ConditionalCommand(onTrue, onFalse) {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}
+
+void MockConditionalCommand::SetCondition(bool condition) {
+  m_condition = condition;
+}
+
+bool MockConditionalCommand::Condition() { return m_condition; }
+
+bool MockConditionalCommand::HasInitialized() {
+  return GetInitializeCount() > 0;
+}
+
+bool MockConditionalCommand::HasEnd() { return GetEndCount() > 0; }
+
+bool MockConditionalCommand::HasInterrupted() {
+  return GetInterruptedCount() > 0;
+}
+
+void MockConditionalCommand::Initialize() { ++m_initializeCount; }
+
+void MockConditionalCommand::Execute() { ++m_executeCount; }
+
+bool MockConditionalCommand::IsFinished() {
+  ++m_isFinishedCount;
+  return ConditionalCommand::IsFinished();
+}
+
+void MockConditionalCommand::End() { ++m_endCount; }
+
+void MockConditionalCommand::Interrupted() { ++m_interruptedCount; }
+
+void MockConditionalCommand::ResetCounters() {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}

wpilibcIntegrationTests/src/main/native/cpp/main.cpp

@@ -0,0 +1,14 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "MockDS.h"
+
+#include <stdint.h>
+
+#include <HAL/cpp/fpga_clock.h>
+#include <llvm/SmallString.h>
+#include <llvm/SmallVector.h>
+#include <llvm/raw_ostream.h>
+#include <support/Logger.h>
+
+#include "udpsockets/UDPClient.h"
+
+static void LoggerFunc(unsigned int level, const char* file, unsigned int line,
+                       const char* msg) {
+  llvm::SmallString<128> buf;
+  llvm::raw_svector_ostream oss(buf);
+  if (level == 20) {
+    oss << "DS: " << msg << '\n';
+    llvm::errs() << oss.str();
+    return;
+  }
+
+  llvm::StringRef levelmsg;
+  if (level >= 50)
+    levelmsg = "CRITICAL: ";
+  else if (level >= 40)
+    levelmsg = "ERROR: ";
+  else if (level >= 30)
+    levelmsg = "WARNING: ";
+  else
+    return;
+  oss << "DS: " << levelmsg << msg << " (" << file << ':' << line << ")\n";
+  llvm::errs() << oss.str();
+}
+
+static void generateEnabledDsPacket(llvm::SmallVectorImpl<uint8_t>& data,
+                                    uint16_t sendCount) {
+  data.clear();
+  data.push_back(sendCount >> 8);
+  data.push_back(sendCount);
+  data.push_back(0x01);  // general data tag
+  data.push_back(0x04);  // teleop enabled
+  data.push_back(0x10);  // normal data request
+  data.push_back(0x00);  // red 1 station
+}
+
+using namespace frc;
+
+void MockDS::start() {
+  if (m_active) return;
+  m_active = true;
+  m_thread = std::thread([&]() {
+    wpi::Logger logger(LoggerFunc);
+    wpi::UDPClient client(logger);
+    client.start();
+    auto timeout_time = hal::fpga_clock::now();
+    int initCount = 0;
+    uint16_t sendCount = 0;
+    llvm::SmallVector<uint8_t, 8> data;
+    while (m_active) {
+      // Keep 20ms intervals, and increase time to next interval
+      auto current = hal::fpga_clock::now();
+      while (timeout_time <= current) {
+        timeout_time += std::chrono::milliseconds(20);
+      }
+      std::this_thread::sleep_until(timeout_time);
+      generateEnabledDsPacket(data, sendCount++);
+      // ~10 disabled packets are required to make the robot actually enable
+      // 1 is definitely not enough.
+      if (initCount < 10) {
+        initCount++;
+        data[3] = 0;
+      }
+      client.send(data, "127.0.0.1", 1110);
+    }
+    client.shutdown();
+  });
+}
+
+void MockDS::stop() {
+  m_active = false;
+  if (m_thread.joinable()) m_thread.join();
+}

wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.h

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+#include <thread>
+
+namespace frc {
+class MockDS {
+ public:
+  MockDS() = default;
+  ~MockDS() { stop(); }
+  MockDS(const MockDS& other) = delete;
+  MockDS& operator=(const MockDS& other) = delete;
+
+  void start();
+  void stop();
+
+ private:
+  std::thread m_thread;
+  std::atomic_bool m_active{false};
+};
+}  // namespace frc