diff --git a/wpilibcIntegrationTests/src/AnalogLoopTest.cpp b/wpilibcIntegrationTests/src/AnalogLoopTest.cpp
new file mode 100644
index 0000000..e8bc1e5
--- /dev/null
+++ b/wpilibcIntegrationTests/src/AnalogLoopTest.cpp
@@ -0,0 +1,128 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+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;
+
+  virtual void SetUp() override {
+    m_input = new AnalogInput(TestBench::kFakeAnalogOutputChannel);
+    m_output = new AnalogOutput(TestBench::kAnalogOutputChannel);
+  }
+
+  virtual 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 (int i = 0; i < 50; i++) {
+    m_output->SetVoltage(i / 10.0f);
+
+    Wait(kDelayTime);
+
+    EXPECT_NEAR(m_output->GetVoltage(), m_input->GetVoltage(), 0.01f);
+  }
+}
+
+/**
+ * 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.0f, 3.0f);
+
+  m_output->SetVoltage(1.0f);
+  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.5f);
+  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.0f);
+  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.0f, 3.0f);
+
+  Counter counter(trigger);
+
+  // Turn the analog output low and high 50 times
+  for (int 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) {
+  *(int *)param = 12345;
+}
+
+TEST_F(AnalogLoopTest, AsynchronusInterruptWorks) {
+  int param = 0;
+  AnalogTrigger trigger(m_input);
+  trigger.SetLimitsVoltage(2.0f, 3.0f);
+
+  // Given an interrupt handler that sets an int to 12345
+  std::shared_ptr<AnalogTriggerOutput> triggerOutput = trigger.CreateOutput(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 int should be 12345
+  Wait(kDelayTime);
+  EXPECT_EQ(12345, param) << "The interrupt did not run.";
+}
diff --git a/wpilibcIntegrationTests/src/AnalogPotentiometerTest.cpp b/wpilibcIntegrationTests/src/AnalogPotentiometerTest.cpp
new file mode 100644
index 0000000..70ca4e8
--- /dev/null
+++ b/wpilibcIntegrationTests/src/AnalogPotentiometerTest.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <AnalogOutput.h>
+#include <AnalogPotentiometer.h>
+#include <Timer.h>
+#include <ControllerPower.h>
+#include "TestBench.h"
+#include "gtest/gtest.h"
+
+static const double kScale = 270.0;
+static const double kAngle = 180.0;
+
+class AnalogPotentiometerTest : public testing::Test {
+ protected:
+  AnalogOutput *m_fakePot;
+  AnalogPotentiometer *m_pot;
+
+  virtual void SetUp() override {
+    m_fakePot = new AnalogOutput(TestBench::kAnalogOutputChannel);
+    m_pot =
+        new AnalogPotentiometer(TestBench::kFakeAnalogOutputChannel, kScale);
+  }
+
+  virtual 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 * ControllerPower::GetVoltage5V());
+  Wait(0.1);
+  EXPECT_NEAR(kAngle, m_pot->Get(), 2.0)
+      << "The potentiometer did not measure the correct angle.";
+}
diff --git a/wpilibcIntegrationTests/src/BuiltInAccelerometerTest.cpp b/wpilibcIntegrationTests/src/BuiltInAccelerometerTest.cpp
new file mode 100644
index 0000000..d6563e0
--- /dev/null
+++ b/wpilibcIntegrationTests/src/BuiltInAccelerometerTest.cpp
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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>
+#include <Timer.h>
+#include "gtest/gtest.h"
+
+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);
+}
diff --git a/wpilibcIntegrationTests/src/CANJaguarTest.cpp b/wpilibcIntegrationTests/src/CANJaguarTest.cpp
new file mode 100644
index 0000000..f1b8e5d
--- /dev/null
+++ b/wpilibcIntegrationTests/src/CANJaguarTest.cpp
@@ -0,0 +1,499 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <AnalogOutput.h>
+#include <DigitalOutput.h>
+#include <CANJaguar.h>
+#include <Relay.h>
+#include <Timer.h>
+#include <WPIErrors.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+static constexpr double kSpikeTime = 0.5;
+
+static constexpr double kExpectedBusVoltage = 14.0;
+static constexpr double kExpectedTemperature = 25.0;
+
+static constexpr double kMotorTime = 0.5;
+
+static constexpr double kEncoderSettlingTime = 1.0;
+static constexpr double kEncoderPositionTolerance = 0.1;
+static constexpr double kEncoderSpeedTolerance = 30.0;
+
+static constexpr double kPotentiometerSettlingTime = 1.0;
+static constexpr double kPotentiometerPositionTolerance = 0.1;
+
+static constexpr double kCurrentTolerance = 0.1;
+
+static constexpr double kVoltageTolerance = 0.1;
+
+static constexpr double kMotorVoltage = 5.0;
+
+static constexpr double kMotorPercent = 0.5;
+
+static constexpr double kMotorSpeed = 100;
+class CANJaguarTest : public testing::Test {
+ protected:
+  CANJaguar *m_jaguar;
+  DigitalOutput *m_fakeForwardLimit, *m_fakeReverseLimit;
+  AnalogOutput *m_fakePotentiometer;
+  Relay *m_spike;
+
+  virtual void SetUp() override {
+    m_spike = new Relay(TestBench::kCANJaguarRelayChannel, Relay::kForwardOnly);
+    m_spike->Set(Relay::kOn);
+    Wait(kSpikeTime);
+
+    m_jaguar = new CANJaguar(TestBench::kCANJaguarID);
+
+    m_fakeForwardLimit = new DigitalOutput(TestBench::kFakeJaguarForwardLimit);
+    m_fakeForwardLimit->Set(0);
+
+    m_fakeReverseLimit = new DigitalOutput(TestBench::kFakeJaguarReverseLimit);
+    m_fakeReverseLimit->Set(0);
+
+    m_fakePotentiometer = new AnalogOutput(TestBench::kFakeJaguarPotentiometer);
+    m_fakePotentiometer->SetVoltage(0.0f);
+
+    /* The motor might still have momentum from the previous test. */
+    Wait(kEncoderSettlingTime);
+  }
+
+  virtual void TearDown() override {
+    delete m_jaguar;
+    delete m_fakeForwardLimit;
+    delete m_fakeReverseLimit;
+    delete m_fakePotentiometer;
+    delete m_spike;
+  }
+
+  /**
+   * Calls CANJaguar::Set periodically 50 times to make sure everything is
+   * verified.  This mimics a real robot program, where Set is presumably
+   * called in each iteration of the main loop.
+   */
+  void SetJaguar(float totalTime, float value = 0.0f) {
+    for (int i = 0; i < 50; i++) {
+      m_jaguar->Set(value);
+      Wait(totalTime / 50.0);
+    }
+  }
+  /**
+  * returns the sign of the given number
+  */
+  int SignNum(double value) { return -(value < 0) + (value > 0); }
+  void InversionTest(float motorValue, float delayTime = kMotorTime) {
+    m_jaguar->EnableControl();
+    m_jaguar->SetInverted(false);
+    SetJaguar(delayTime, motorValue);
+    double initialSpeed = m_jaguar->GetSpeed();
+    m_jaguar->Set(0.0);
+    m_jaguar->SetInverted(true);
+    SetJaguar(delayTime, motorValue);
+    double finalSpeed = m_jaguar->GetSpeed();
+    // checks that the motor has changed direction
+    EXPECT_FALSE(SignNum(initialSpeed) == SignNum(finalSpeed))
+        << "CAN Jaguar did not invert direction positive. Initial speed was: "
+        << initialSpeed << " Final displacement was: " << finalSpeed
+        << " Sign of initial displacement was: " << SignNum(initialSpeed)
+        << " Sign of final displacement was: " << SignNum(finalSpeed);
+    SetJaguar(delayTime, -motorValue);
+    initialSpeed = m_jaguar->GetSpeed();
+    m_jaguar->Set(0.0);
+    m_jaguar->SetInverted(false);
+    SetJaguar(delayTime, -motorValue);
+    finalSpeed = m_jaguar->GetSpeed();
+    EXPECT_FALSE(SignNum(initialSpeed) == SignNum(finalSpeed))
+        << "CAN Jaguar did not invert direction negative. Initial displacement "
+           "was: " << initialSpeed << " Final displacement was: " << finalSpeed
+        << " Sign of initial displacement was: " << SignNum(initialSpeed)
+        << " Sign of final displacement was: " << SignNum(finalSpeed);
+  }
+};
+
+/**
+ * Tests that allocating the same CANJaguar port as an already allocated port
+ * causes a ResourceAlreadyAllocated error.
+ */
+TEST_F(CANJaguarTest, AlreadyAllocatedError) {
+  std::cout << "The following errors are expected." << std::endl
+            << std::endl;
+
+  CANJaguar jaguar(TestBench::kCANJaguarID);
+  EXPECT_EQ(wpi_error_value_ResourceAlreadyAllocated,
+            jaguar.GetError().GetCode())
+      << "An error should have been returned";
+}
+
+/**
+ * Test that allocating a CANJaguar with device number 64 causes an
+ * out-of-range error.
+ */
+TEST_F(CANJaguarTest, 64OutOfRangeError) {
+  std::cout << "The following errors are expected." << std::endl
+            << std::endl;
+
+  CANJaguar jaguar(64);
+  EXPECT_EQ(wpi_error_value_ChannelIndexOutOfRange, jaguar.GetError().GetCode())
+      << "An error should have been returned";
+}
+
+/**
+ * Test that allocating a CANJaguar with device number 0 causes an out-of-range
+ * error.
+ */
+TEST_F(CANJaguarTest, 0OutOfRangeError) {
+  std::cout << "The following errors are expected." << std::endl
+            << std::endl;
+
+  CANJaguar jaguar(0);
+  EXPECT_EQ(wpi_error_value_ChannelIndexOutOfRange, jaguar.GetError().GetCode())
+      << "An error should have been returned";
+}
+
+/**
+ * Checks the default status data for reasonable values to confirm that we're
+ * really getting status data from the Jaguar.
+ */
+TEST_F(CANJaguarTest, InitialStatus) {
+  m_jaguar->SetPercentMode();
+
+  EXPECT_NEAR(m_jaguar->GetBusVoltage(), kExpectedBusVoltage, 3.0)
+      << "Bus voltage is not a plausible value.";
+
+  EXPECT_FLOAT_EQ(m_jaguar->GetOutputVoltage(), 0.0)
+      << "Output voltage is non-zero.";
+
+  EXPECT_FLOAT_EQ(m_jaguar->GetOutputCurrent(), 0.0)
+      << "Output current is non-zero.";
+
+  EXPECT_NEAR(m_jaguar->GetTemperature(), kExpectedTemperature, 5.0)
+      << "Temperature is not a plausible value.";
+
+  EXPECT_EQ(m_jaguar->GetFaults(), 0) << "Jaguar has one or more fault set.";
+}
+
+/**
+ * Ensure that the jaguar doesn't move when it's disabled
+ */
+TEST_F(CANJaguarTest, Disable) {
+  m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
+  m_jaguar->EnableControl();
+  m_jaguar->DisableControl();
+
+  Wait(kEncoderSettlingTime);
+
+  double initialPosition = m_jaguar->GetPosition();
+
+  SetJaguar(kMotorTime, 1.0f);
+  m_jaguar->Set(0.0f);
+
+  double finalPosition = m_jaguar->GetPosition();
+
+  EXPECT_NEAR(initialPosition, finalPosition, kEncoderPositionTolerance)
+      << "Jaguar moved while disabled";
+}
+
+/**
+ * Make sure the Jaguar keeps its state after a power cycle by setting a
+ * control mode, turning the spike on and off, then checking if the Jaguar
+ * behaves like it should in that control mode.
+ */
+TEST_F(CANJaguarTest, BrownOut) {
+  /* Set the jaguar to quad encoder position mode */
+  m_jaguar->SetPositionMode(CANJaguar::QuadEncoder, 360, 20.0f, 0.01f, 0.0f);
+  m_jaguar->EnableControl();
+  SetJaguar(kMotorTime, 0.0);
+  double setpoint = m_jaguar->GetPosition() + 1.0f;
+
+  /* Turn the spike off and on again */
+  m_spike->Set(Relay::kOff);
+  Wait(kSpikeTime);
+  m_spike->Set(Relay::kOn);
+  Wait(kSpikeTime);
+
+  /* The jaguar should automatically get set to quad encoder position mode,
+          so it should be able to reach a setpoint in a couple seconds. */
+  for (int i = 0; i < 10; i++) {
+    SetJaguar(1.0f, setpoint);
+
+    if (std::abs(m_jaguar->GetPosition() - setpoint) <=
+        kEncoderPositionTolerance) {
+      return;
+    }
+  }
+
+  EXPECT_NEAR(setpoint, m_jaguar->GetPosition(), kEncoderPositionTolerance)
+      << "CAN Jaguar should have resumed PID control after power cycle";
+}
+
+/**
+ * Test if we can set arbitrary setpoints and PID values each each applicable
+ * mode and get the same values back.
+ */
+TEST_F(CANJaguarTest, SetGet) {
+  m_jaguar->DisableControl();
+
+  m_jaguar->SetSpeedMode(CANJaguar::QuadEncoder, 360, 1, 2, 3);
+  m_jaguar->Set(4);
+
+  EXPECT_FLOAT_EQ(1, m_jaguar->GetP());
+  EXPECT_FLOAT_EQ(2, m_jaguar->GetI());
+  EXPECT_FLOAT_EQ(3, m_jaguar->GetD());
+  EXPECT_FLOAT_EQ(4, m_jaguar->Get());
+}
+
+/**
+ * Test if we can drive the motor in percentage mode and get a position back
+ */
+TEST_F(CANJaguarTest, PercentModeForwardWorks) {
+  m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
+  m_jaguar->EnableControl();
+
+  /* The motor might still have momentum from the previous test. */
+  SetJaguar(kEncoderSettlingTime, 0.0f);
+
+  double initialPosition = m_jaguar->GetPosition();
+
+  /* Drive the speed controller briefly to move the encoder */
+  SetJaguar(kMotorTime, 1.0f);
+  m_jaguar->Set(0.0f);
+
+  /* The position should have increased */
+  EXPECT_GT(m_jaguar->GetPosition(), initialPosition)
+      << "CAN Jaguar position should have increased after the motor moved";
+}
+
+/**
+ * Test if we can drive the motor backwards in percentage mode and get a
+ * position back
+ */
+TEST_F(CANJaguarTest, PercentModeReverseWorks) {
+  m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
+  m_jaguar->EnableControl();
+
+  /* The motor might still have momentum from the previous test. */
+  SetJaguar(kEncoderSettlingTime, 0.0f);
+
+  double initialPosition = m_jaguar->GetPosition();
+
+  /* Drive the speed controller briefly to move the encoder */
+  SetJaguar(kMotorTime, -1.0f);
+  m_jaguar->Set(0.0f);
+
+  float p = m_jaguar->GetPosition();
+  /* The position should have decreased */
+  EXPECT_LT(p, initialPosition)
+      << "CAN Jaguar position should have decreased after the motor moved";
+}
+
+/**
+ * Test if we can set an absolute voltage and receive a matching output voltage
+ * status.
+ */
+TEST_F(CANJaguarTest, VoltageModeWorks) {
+  m_jaguar->SetVoltageMode();
+  m_jaguar->EnableControl();
+
+  float setpoints[] = {M_PI, 8.0f, -10.0f};
+
+  for (auto setpoint : setpoints) {
+    SetJaguar(kMotorTime, setpoint);
+    EXPECT_NEAR(setpoint, m_jaguar->GetOutputVoltage(), kVoltageTolerance);
+  }
+}
+
+/**
+ * Test if we can set a speed in speed control mode and receive a matching
+ * speed status.
+ */
+TEST_F(CANJaguarTest, SpeedModeWorks) {
+  m_jaguar->SetSpeedMode(CANJaguar::QuadEncoder, 360, 0.1f, 0.003f, 0.01f);
+  m_jaguar->EnableControl();
+
+  constexpr float speed = 50.0f;
+
+  SetJaguar(kMotorTime, speed);
+  EXPECT_NEAR(speed, m_jaguar->GetSpeed(), kEncoderSpeedTolerance);
+}
+
+/**
+ * Test if we can set a position and reach that position with PID control on
+ * the Jaguar.
+ */
+TEST_F(CANJaguarTest, PositionModeWorks) {
+  m_jaguar->SetPositionMode(CANJaguar::QuadEncoder, 360, 15.0f, 0.02f, 0.0f);
+
+  double setpoint = m_jaguar->GetPosition() + 1.0f;
+
+  m_jaguar->EnableControl();
+
+  /* It should get to the setpoint within 10 seconds */
+  for (int i = 0; i < 10; i++) {
+    SetJaguar(1.0f, setpoint);
+
+    if (std::abs(m_jaguar->GetPosition() - setpoint) <=
+        kEncoderPositionTolerance) {
+      return;
+    }
+  }
+
+  EXPECT_NEAR(setpoint, m_jaguar->GetPosition(), kEncoderPositionTolerance)
+      << "CAN Jaguar should have reached setpoint with PID control";
+}
+
+/**
+ * Test if we can set a current setpoint with PID control on the Jaguar and get
+ * a corresponding output current
+ */
+TEST_F(CANJaguarTest, DISABLED_CurrentModeWorks) {
+  m_jaguar->SetCurrentMode(10.0, 4.0, 1.0);
+  m_jaguar->EnableControl();
+
+  float setpoints[] = {1.6f, 2.0f, -1.6f};
+
+  for (auto& setpoints_i : setpoints) {
+    float setpoint = setpoints_i;
+    float expectedCurrent = std::abs(setpoints_i);
+
+    /* It should get to each setpoint within 10 seconds */
+    for (int j = 0; j < 10; j++) {
+      SetJaguar(1.0, setpoint);
+
+      if (std::abs(m_jaguar->GetOutputCurrent() - expectedCurrent) <=
+          kCurrentTolerance) {
+        break;
+      }
+    }
+
+    EXPECT_NEAR(expectedCurrent, m_jaguar->GetOutputCurrent(),
+                kCurrentTolerance);
+  }
+}
+
+/**
+ * Test if we can get a position in potentiometer mode, using an analog output
+ * as a fake potentiometer.
+ */
+TEST_F(CANJaguarTest, FakePotentiometerPosition) {
+  m_jaguar->SetPercentMode(CANJaguar::Potentiometer);
+  m_jaguar->EnableControl();
+
+  // Set the analog output to 4 different voltages and check if the Jaguar
+  // returns corresponding positions.
+  for (int i = 0; i <= 3; i++) {
+    m_fakePotentiometer->SetVoltage(static_cast<float>(i));
+
+    SetJaguar(kPotentiometerSettlingTime);
+
+    EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f,
+                m_jaguar->GetPosition(), kPotentiometerPositionTolerance)
+        << "CAN Jaguar should have returned the potentiometer position set by "
+           "the analog output";
+  }
+}
+
+/**
+ * Test if we can limit the Jaguar to only moving in reverse with a fake
+ * limit switch.
+ */
+TEST_F(CANJaguarTest, FakeLimitSwitchForwards) {
+  m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
+  m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly);
+  m_fakeForwardLimit->Set(1);
+  m_fakeReverseLimit->Set(0);
+  m_jaguar->EnableControl();
+
+  SetJaguar(kEncoderSettlingTime);
+
+  /* Make sure the limits are recognized by the Jaguar. */
+  ASSERT_FALSE(m_jaguar->GetForwardLimitOK());
+  ASSERT_TRUE(m_jaguar->GetReverseLimitOK());
+
+  double initialPosition = m_jaguar->GetPosition();
+
+  /* Drive the speed controller briefly to move the encoder.  If the limit
+           switch is recognized, it shouldn't actually move. */
+  SetJaguar(kMotorTime, 1.0f);
+
+  /* The position should be the same, since the limit switch was on. */
+  EXPECT_NEAR(initialPosition, m_jaguar->GetPosition(),
+              kEncoderPositionTolerance)
+      << "CAN Jaguar should not have moved with the limit switch pressed";
+
+  /* Drive the speed controller in the other direction.  It should actually
+           move, since only the forward switch is activated.*/
+  SetJaguar(kMotorTime, -1.0f);
+
+  /* The position should have decreased */
+  EXPECT_LT(m_jaguar->GetPosition(), initialPosition)
+      << "CAN Jaguar should have moved in reverse while the forward limit was "
+         "on";
+}
+
+/**
+ * Test if we can limit the Jaguar to only moving forwards with a fake limit
+ * switch.
+ */
+TEST_F(CANJaguarTest, FakeLimitSwitchReverse) {
+  m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
+  m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly);
+  m_fakeForwardLimit->Set(0);
+  m_fakeReverseLimit->Set(1);
+  m_jaguar->EnableControl();
+
+  SetJaguar(kEncoderSettlingTime);
+
+  /* Make sure the limits are recognized by the Jaguar. */
+  ASSERT_TRUE(m_jaguar->GetForwardLimitOK());
+  ASSERT_FALSE(m_jaguar->GetReverseLimitOK());
+
+  double initialPosition = m_jaguar->GetPosition();
+
+  /* Drive the speed controller backwards briefly to move the encoder.  If
+           the limit switch is recognized, it shouldn't actually move. */
+  SetJaguar(kMotorTime, -1.0f);
+
+  /* The position should be the same, since the limit switch was on. */
+  EXPECT_NEAR(initialPosition, m_jaguar->GetPosition(),
+              kEncoderPositionTolerance)
+      << "CAN Jaguar should not have moved with the limit switch pressed";
+
+  /* Drive the speed controller in the other direction.  It should actually
+           move, since only the reverse switch is activated.*/
+  SetJaguar(kMotorTime, 1.0f);
+
+  /* The position should have increased */
+  EXPECT_GT(m_jaguar->GetPosition(), initialPosition)
+      << "CAN Jaguar should have moved forwards while the reverse limit was on";
+}
+/**
+* Tests that inversion works in voltage mode
+*/
+TEST_F(CANJaguarTest, InvertingVoltageMode) {
+  m_jaguar->SetVoltageMode(CANJaguar::QuadEncoder, 360);
+  m_jaguar->EnableControl();
+  InversionTest(kMotorVoltage);
+}
+
+/**
+* Tests that inversion works in percentMode
+*/
+TEST_F(CANJaguarTest, InvertingPercentMode) {
+  m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360);
+  m_jaguar->EnableControl();
+  InversionTest(kMotorPercent);
+}
+/**
+* Tests that inversion works in SpeedMode
+*/
+TEST_F(CANJaguarTest, InvertingSpeedMode) {
+  m_jaguar->SetSpeedMode(CANJaguar::QuadEncoder, 360, 0.1f, 0.005f, 0.00f);
+  m_jaguar->EnableControl();
+  InversionTest(kMotorSpeed, kMotorTime);
+}
diff --git a/wpilibcIntegrationTests/src/CANTalonTest.cpp b/wpilibcIntegrationTests/src/CANTalonTest.cpp
new file mode 100644
index 0000000..2584f3f
--- /dev/null
+++ b/wpilibcIntegrationTests/src/CANTalonTest.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include <CANTalon.h>
+#include <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+const int deviceId = 0;
+
+TEST(CANTalonTest, QuickTest) {
+  double throttle = 0.1;
+  CANTalon talon(deviceId);
+  talon.SetControlMode(CANSpeedController::kPercentVbus);
+  talon.EnableControl();
+  talon.Set(throttle);
+  Wait(0.25);
+  EXPECT_NEAR(talon.Get(), throttle, 5e-3);
+  talon.Set(-throttle);
+  Wait(0.25);
+  EXPECT_NEAR(talon.Get(), -throttle, 5e-3);
+
+  talon.Disable();
+  Wait(0.1);
+  EXPECT_FLOAT_EQ(talon.Get(), 0.0);
+}
+
+TEST(CANTalonTest, SetGetPID) {
+  // Tests that we can actually set and get PID values as intended.
+  CANTalon talon(deviceId);
+  double p = 0.05, i = 0.098, d = 1.23;
+  talon.SetPID(p, i, d);
+  // Wait(0.03);
+  EXPECT_NEAR(p, talon.GetP(), 1e-5);
+  EXPECT_NEAR(i, talon.GetI(), 1e-5);
+  EXPECT_NEAR(d, talon.GetD(), 1e-5);
+  // Test with new values in case the talon was already set to the previous
+  // ones.
+  p = 0.15;
+  i = 0.198;
+  d = 1.03;
+  talon.SetPID(p, i, d);
+  // Wait(0.03);
+  EXPECT_NEAR(p, talon.GetP(), 1e-5);
+  EXPECT_NEAR(i, talon.GetI(), 1e-5);
+  EXPECT_NEAR(d, talon.GetD(), 1e-5);
+}
+
+TEST(CANTalonTest, DISABLED_PositionModeWorks) {
+  CANTalon talon(deviceId);
+  talon.SetFeedbackDevice(CANTalon::AnalogPot);
+  talon.SetControlMode(CANSpeedController::kPosition);
+  Wait(0.1);
+  double p = 2;
+  double i = 0.00;
+  double d = 0.00;
+  Wait(0.2);
+  talon.SetControlMode(CANSpeedController::kPosition);
+  talon.SetFeedbackDevice(CANTalon::AnalogPot);
+  talon.SetPID(p, i, d);
+  Wait(0.2);
+  talon.Set(100);
+  Wait(100);
+  talon.Disable();
+  EXPECT_NEAR(talon.Get(), 500, 1000);
+}
diff --git a/wpilibcIntegrationTests/src/ConditionVariableTest.cpp b/wpilibcIntegrationTests/src/ConditionVariableTest.cpp
new file mode 100644
index 0000000..43e792a
--- /dev/null
+++ b/wpilibcIntegrationTests/src/ConditionVariableTest.cpp
@@ -0,0 +1,294 @@
+#include "TestBench.h"
+
+#include "HAL/cpp/priority_condition_variable.h"
+#include "HAL/cpp/priority_mutex.h"
+
+#include "gtest/gtest.h"
+#include <atomic>
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+
+namespace wpilib {
+namespace testing {
+
+// Tests that the condition variable class which we wrote ourselves actually
+// does work.
+class ConditionVariableTest : public ::testing::Test {
+ protected:
+  typedef std::unique_lock<priority_mutex> priority_lock;
+
+  // Condition variable to test.
+  priority_condition_variable m_cond;
+
+  // Mutex to pass to condition variable when waiting.
+  priority_mutex m_mutex;
+
+  // flags for testing when threads are completed.
+  std::atomic<bool> m_done1{false}, m_done2{false};
+  // Threads to use for testing. We want multiple threads to ensure that it
+  // behaves correctly when multiple processes are waiting on a signal.
+  std::thread m_watcher1, m_watcher2;
+
+  // Information for when running with predicates.
+  std::atomic<bool> m_pred_var{false};
+
+  void ShortSleep(unsigned long time=10) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(time));
+  }
+
+  // Start up the given threads with a wait function. The wait function should
+  // call some version of m_cond.wait and should take as an argument a reference
+  // to an std::atomic<bool> which it will set to true when it is ready to have
+  // join called on it.
+  template <class Function>
+  void StartThreads(Function wait) {
+    m_watcher1 = std::thread(wait, std::ref(m_done1));
+    m_watcher2 = std::thread(wait, std::ref(m_done2));
+
+    // Wait briefly to let the lock be unlocked.
+    ShortSleep();
+    bool locked = m_mutex.try_lock();
+    if (locked) m_mutex.unlock();
+    EXPECT_TRUE(locked)
+        << "The condition variable failed to unlock the lock.";
+  }
+
+  void NotifyAll() { m_cond.notify_all(); }
+  void NotifyOne() { m_cond.notify_one(); }
+
+  // Test that all the threads are notified by a notify_all() call.
+  void NotifyAllTest() {
+    NotifyAll();
+    // Wait briefly to let the lock be re-locked.
+    ShortSleep();
+    EXPECT_TRUE(m_done1) << "watcher1 failed to be notified.";
+    EXPECT_TRUE(m_done2) << "watcher2 failed to be notified.";
+  }
+
+  // For use when testing predicates. First tries signalling the threads with
+  // the predicate set to false (and ensures that they do not activate) and then
+  // tests with the predicate set to true.
+  void PredicateTest() {
+    m_pred_var = false;
+    NotifyAll();
+    ShortSleep();
+    EXPECT_FALSE(m_done1) << "watcher1 didn't pay attention to its predicate.";
+    EXPECT_FALSE(m_done2) << "watcher2 didn't pay attention to its predicate.";
+    m_pred_var = true;
+    NotifyAllTest();
+  }
+
+  // Used by the WaitFor and WaitUntil tests to test that, without a predicate,
+  // the timeout works properly.
+  void WaitTimeTest(bool wait_for) {
+    std::atomic<bool> timed_out{true};
+    auto wait_until = [this, &timed_out, wait_for](std::atomic<bool> &done) {
+      priority_lock lock(m_mutex);
+      done = false;
+      if (wait_for) {
+        auto wait_time = std::chrono::milliseconds(100);
+        timed_out =
+            m_cond.wait_for(lock, wait_time) == std::cv_status::timeout;
+      } else {
+        auto wait_time =
+            std::chrono::system_clock::now() + std::chrono::milliseconds(100);
+        timed_out =
+            m_cond.wait_until(lock, wait_time) == std::cv_status::timeout;
+      }
+      EXPECT_TRUE(lock.owns_lock())
+          << "The condition variable should have reacquired the lock.";
+      done = true;
+    };
+
+    // First, test without timing out.
+    timed_out = true;
+    StartThreads(wait_until);
+
+    NotifyAllTest();
+    EXPECT_FALSE(timed_out) << "The watcher should not have timed out.";
+
+    TearDown();
+
+    // Next, test and time out.
+    timed_out = false;
+    StartThreads(wait_until);
+
+    ShortSleep(110);
+
+    EXPECT_TRUE(m_done1) << "watcher1 should have timed out.";
+    EXPECT_TRUE(m_done2) << "watcher2 should have timed out.";
+    EXPECT_TRUE(timed_out) << "The watcher should have timed out.";
+  }
+
+  // For use with tests that have a timeout and a predicate.
+  void WaitTimePredicateTest(bool wait_for) {
+    // The condition_variable return value from the wait_for or wait_until
+    // function should in the case of having a predicate, by a boolean. If the
+    // predicate is true, then the return value will always be true. If the
+    // condition times out and, at the time of the timeout, the predicate is
+    // false, the return value will be false.
+    std::atomic<bool> retval{true};
+    auto predicate = [this]() -> bool { return m_pred_var; };
+    auto wait_until =
+        [this, &retval, predicate, wait_for](std::atomic<bool> &done) {
+      priority_lock lock(m_mutex);
+      done = false;
+      if (wait_for) {
+        auto wait_time = std::chrono::milliseconds(100);
+        retval = m_cond.wait_for(lock, wait_time, predicate);
+      } else {
+        auto wait_time =
+            std::chrono::system_clock::now() + std::chrono::milliseconds(100);
+        retval = m_cond.wait_until(lock, wait_time, predicate);
+      }
+      EXPECT_TRUE(lock.owns_lock())
+          << "The condition variable should have reacquired the lock.";
+      done = true;
+    };
+
+    // Test without timing out and with the predicate set to true.
+    retval = true;
+    m_pred_var = true;
+    StartThreads(wait_until);
+
+    NotifyAllTest();
+    EXPECT_TRUE(retval) << "The watcher should not have timed out.";
+
+    TearDown();
+
+    // Test with timing out and with the predicate set to true.
+    retval = false;
+    m_pred_var = false;
+    StartThreads(wait_until);
+
+    ShortSleep(110);
+
+    EXPECT_TRUE(m_done1) << "watcher1 should have finished.";
+    EXPECT_TRUE(m_done2) << "watcher2 should have finished.";
+    EXPECT_FALSE(retval) << "The watcher should have timed out.";
+
+    TearDown();
+
+    // Test without timing out and run the PredicateTest().
+    retval = false;
+    StartThreads(wait_until);
+
+    PredicateTest();
+    EXPECT_TRUE(retval) << "The return value should have been true.";
+
+    TearDown();
+
+    // Test with timing out and the predicate set to true while we are waiting
+    // for the condition variable to time out.
+    retval = true;
+    StartThreads(wait_until);
+    ShortSleep();
+    m_pred_var = true;
+    ShortSleep(110);
+    EXPECT_TRUE(retval) << "The return value should have been true.";
+  }
+
+  virtual void TearDown() {
+    // If a thread has not completed, then continuing will cause the tests to
+    // hang forever and could cause issues. If we don't call detach, then
+    // std::terminate is called and all threads are terminated.
+    // Detaching is non-optimal, but should allow the rest of the tests to run
+    // before anything drastic occurs.
+    if (m_done1) m_watcher1.join();
+    else m_watcher1.detach();
+    if (m_done2) m_watcher2.join();
+    else m_watcher2.detach();
+  }
+};
+
+TEST_F(ConditionVariableTest, NotifyAll) {
+  auto wait = [this](std::atomic<bool> &done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  NotifyAllTest();
+}
+
+TEST_F(ConditionVariableTest, NotifyOne) {
+  auto wait = [this](std::atomic<bool> &done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  NotifyOne();
+  // Wait briefly to let things settle.
+  ShortSleep();
+  EXPECT_TRUE(m_done1 ^ m_done2) << "Only one thread should've been notified.";
+  NotifyOne();
+  ShortSleep();
+  EXPECT_TRUE(m_done2 && m_done2) << "Both threads should've been notified.";
+}
+
+TEST_F(ConditionVariableTest, WaitWithPredicate) {
+  auto predicate = [this]() -> bool { return m_pred_var; };
+  auto wait_predicate = [this, predicate](std::atomic<bool> &done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock, predicate);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait_predicate);
+
+  PredicateTest();
+}
+
+TEST_F(ConditionVariableTest, WaitUntil) {
+  WaitTimeTest(false);
+}
+
+TEST_F(ConditionVariableTest, WaitUntilWithPredicate) {
+  WaitTimePredicateTest(false);
+}
+
+TEST_F(ConditionVariableTest, WaitFor) {
+  WaitTimeTest(true);
+}
+
+TEST_F(ConditionVariableTest, WaitForWithPredicate) {
+  WaitTimePredicateTest(true);
+}
+
+TEST_F(ConditionVariableTest, NativeHandle) {
+  auto wait = [this](std::atomic<bool> &done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  pthread_cond_t* native_handle = m_cond.native_handle();
+  pthread_cond_broadcast(native_handle);
+  ShortSleep();
+  EXPECT_TRUE(m_done1) << "watcher1 failed to be notified.";
+  EXPECT_TRUE(m_done2) << "watcher2 failed to be notified.";
+}
+
+}  // namespace testing
+}  // namespace wpilib
diff --git a/wpilibcIntegrationTests/src/CounterTest.cpp b/wpilibcIntegrationTests/src/CounterTest.cpp
new file mode 100644
index 0000000..12a4226
--- /dev/null
+++ b/wpilibcIntegrationTests/src/CounterTest.cpp
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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>
+#include <Jaguar.h>
+#include <Talon.h>
+#include <Timer.h>
+#include <Victor.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+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;
+
+  virtual 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);
+  }
+
+  virtual 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.0f);
+    m_victor->Set(0.0f);
+    m_jaguar->Set(0.0f);
+  }
+};
+
+/**
+ * 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.0f);
+  Wait(0.5);
+  EXPECT_NE(0.0f, 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.0f);
+  Wait(0.5);
+  m_talonCounter->Reset();
+  EXPECT_FLOAT_EQ(0.0f, 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.0f);
+  Wait(0.5);
+  EXPECT_NE(0.0f, 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.0f);
+  Wait(0.5);
+  m_victorCounter->Reset();
+  EXPECT_FLOAT_EQ(0.0f, 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.0f);
+  Wait(0.5);
+  EXPECT_NE(0.0f, 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.0f);
+  Wait(0.5);
+  m_jaguarCounter->Reset();
+  EXPECT_FLOAT_EQ(0.0f, 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.0f);
+  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.0f);
+  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.0f);
+  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.0f);
+  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.0f);
+  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.0f);
+  Wait(kMotorDelay);
+  EXPECT_TRUE(m_jaguarCounter->GetStopped())
+      << "The counter did not stop counting.";
+}
diff --git a/wpilibcIntegrationTests/src/DIOLoopTest.cpp b/wpilibcIntegrationTests/src/DIOLoopTest.cpp
new file mode 100644
index 0000000..c13d145
--- /dev/null
+++ b/wpilibcIntegrationTests/src/DIOLoopTest.cpp
@@ -0,0 +1,127 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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>
+#include <DigitalInput.h>
+#include <DigitalOutput.h>
+#include <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+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;
+
+  virtual void SetUp() override {
+    m_input = new DigitalInput(TestBench::kLoop1InputChannel);
+    m_output = new DigitalOutput(TestBench::kLoop1OutputChannel);
+  }
+
+  virtual 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.";
+}
+
+/**
+ * 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 (int 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) {
+  *(int *)param = 12345;
+}
+
+TEST_F(DIOLoopTest, AsynchronousInterruptWorks) {
+  int param = 0;
+
+  // Given an interrupt handler that sets an int 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 int 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);
+}
diff --git a/wpilibcIntegrationTests/src/DigitalGlitchFilterTest.cpp b/wpilibcIntegrationTests/src/DigitalGlitchFilterTest.cpp
new file mode 100644
index 0000000..dfcec48
--- /dev/null
+++ b/wpilibcIntegrationTests/src/DigitalGlitchFilterTest.cpp
@@ -0,0 +1,60 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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>
+#include <DigitalInput.h>
+#include <Encoder.h>
+#include <DigitalGlitchFilter.h>
+#include "gtest/gtest.h"
+
+/**
+ * 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);
+}
diff --git a/wpilibcIntegrationTests/src/FakeEncoderTest.cpp b/wpilibcIntegrationTests/src/FakeEncoderTest.cpp
new file mode 100644
index 0000000..3a5dbbb
--- /dev/null
+++ b/wpilibcIntegrationTests/src/FakeEncoderTest.cpp
@@ -0,0 +1,159 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <AnalogOutput.h>
+#include <AnalogTrigger.h>
+#include <DigitalOutput.h>
+#include <Encoder.h>
+#include <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+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;
+
+  virtual 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);
+  }
+
+  virtual 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 (int 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.0f, 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.0f, 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());
+}
diff --git a/wpilibcIntegrationTests/src/MotorEncoderTest.cpp b/wpilibcIntegrationTests/src/MotorEncoderTest.cpp
new file mode 100644
index 0000000..47f0ab9
--- /dev/null
+++ b/wpilibcIntegrationTests/src/MotorEncoderTest.cpp
@@ -0,0 +1,185 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <PIDController.h>
+#include <Talon.h>
+#include <Timer.h>
+#include <Victor.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+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.
+ * @author Thomas Clark
+ */
+class MotorEncoderTest : public testing::TestWithParam<MotorEncoderTestType> {
+ protected:
+  SpeedController *m_speedController;
+  Encoder *m_encoder;
+
+  virtual 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;
+    }
+  }
+
+  virtual void TearDown() override {
+    delete m_speedController;
+    delete m_encoder;
+  }
+
+  void Reset() {
+    m_speedController->Set(0.0f);
+    m_encoder->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(1.0);
+  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(-1.0f);
+  Wait(kMotorTime);
+  m_speedController->Set(0.0f);
+
+  /* The encoder should be positive now */
+  EXPECT_LT(m_encoder->Get(), 0.0f)
+      << "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.0f);
+  Wait(kMotorTime);
+
+  EXPECT_FLOAT_EQ(1.0f, m_speedController->Get());
+
+  m_speedController->Set(-2.0f);
+  Wait(kMotorTime);
+
+  EXPECT_FLOAT_EQ(-1.0f, m_speedController->Get());
+}
+
+/**
+ * Test if position PID loop works
+ */
+TEST_P(MotorEncoderTest, PositionPIDController) {
+  Reset();
+
+  m_encoder->SetPIDSourceType(PIDSourceType::kDisplacement);
+  PIDController pid(0.001f, 0.0005f, 0.0f, m_encoder, m_speedController);
+  pid.SetAbsoluteTolerance(20.0f);
+  pid.SetOutputRange(-0.3f, 0.3f);
+  pid.SetSetpoint(2500);
+
+  /* 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.";
+}
+
+/**
+ * Test if velocity PID loop works
+ */
+TEST_P(MotorEncoderTest, VelocityPIDController) {
+  Reset();
+
+  m_encoder->SetPIDSourceType(PIDSourceType::kRate);
+  PIDController pid(1e-5, 0.0f, 3e-5, 8e-5, m_encoder, m_speedController);
+  pid.SetAbsoluteTolerance(50.0f);
+  pid.SetToleranceBuffer(10);
+  pid.SetOutputRange(-0.3f, 0.3f);
+  pid.SetSetpoint(2000);
+
+  /* 10 seconds should be plenty time to get to the setpoint */
+  pid.Enable();
+  Wait(10.0);
+
+  RecordProperty("PIDError", pid.GetAvgError());
+
+  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 2000 << " Error was: " << pid.GetError();
+
+  pid.Disable();
+}
+
+/**
+ * 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));
diff --git a/wpilibcIntegrationTests/src/MotorInvertingTest.cpp b/wpilibcIntegrationTests/src/MotorInvertingTest.cpp
new file mode 100644
index 0000000..1794299
--- /dev/null
+++ b/wpilibcIntegrationTests/src/MotorInvertingTest.cpp
@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <Timer.h>
+#include <Victor.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+enum MotorInvertingTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
+static const double motorSpeed = 0.25;
+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;
+  virtual 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;
+    }
+  }
+  virtual void TearDown() override {
+    delete m_speedController;
+    delete m_encoder;
+  }
+
+  void Reset() {
+    m_speedController->SetInverted(false);
+    m_speedController->Set(0.0f);
+    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));
diff --git a/wpilibcIntegrationTests/src/MutexTest.cpp b/wpilibcIntegrationTests/src/MutexTest.cpp
new file mode 100644
index 0000000..1e80ec9
--- /dev/null
+++ b/wpilibcIntegrationTests/src/MutexTest.cpp
@@ -0,0 +1,261 @@
+#include "HAL/cpp/priority_mutex.h"
+#include "TestBench.h"
+
+#include "gtest/gtest.h"
+#include <atomic>
+#include <condition_variable>
+#include <thread>
+
+namespace wpilib {
+namespace testing {
+
+using std::chrono::system_clock;
+
+// Threading primitive used to notify many threads that a condition is now true.
+// The condition can not be cleared.
+class Notification {
+ public:
+  // Efficiently waits until the Notification has been notified once.
+  void Wait() {
+    std::unique_lock<priority_mutex> lock(m_mutex);
+    while (!m_set) {
+      m_condition.wait(lock);
+    }
+  }
+  // Sets the condition to true, and wakes all waiting threads.
+  void Notify() {
+    std::lock_guard<priority_mutex> lock(m_mutex);
+    m_set = true;
+    m_condition.notify_all();
+  }
+
+ private:
+  // priority_mutex used for the notification and to protect the bit.
+  priority_mutex m_mutex;
+  // Condition for threads to sleep on.
+  std::condition_variable_any m_condition;
+  // Bool which is true when the notification has been notified.
+  bool m_set = false;
+};
+
+void SetProcessorAffinity(int core_id) {
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  CPU_SET(core_id, &cpuset);
+
+  pthread_t current_thread = pthread_self();
+  ASSERT_TRUE(
+      pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &cpuset) == 0);
+}
+
+void SetThreadRealtimePriorityOrDie(int priority) {
+  struct sched_param param;
+  // Set realtime priority for this thread
+  param.sched_priority = priority + sched_get_priority_min(SCHED_RR);
+  ASSERT_TRUE(pthread_setschedparam(pthread_self(), SCHED_RR, &param) == 0)
+      << ": Failed to set scheduler priority.";
+}
+
+// This thread holds the mutex and spins until signaled to release it and stop.
+template <typename MutexType>
+class LowPriorityThread {
+ public:
+  LowPriorityThread(MutexType *mutex)
+      : m_mutex(mutex), m_hold_mutex(1), m_success(0) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(20);
+    m_mutex->lock();
+    m_started.Notify();
+    while (m_hold_mutex.test_and_set()) {}
+    m_mutex->unlock();
+    m_success.store(1);
+  }
+
+  void WaitForStartup() { m_started.Wait(); }
+  void release_mutex() { m_hold_mutex.clear(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  // priority_mutex to grab and release.
+  MutexType *m_mutex;
+  Notification m_started;
+  // Atomic type used to signal when the thread should unlock the mutex.
+  // Using a mutex to protect something could cause other issues, and a delay
+  // between setting and reading isn't a problem as long as the set is atomic.
+  std::atomic_flag m_hold_mutex;
+  std::atomic<int> m_success;
+};
+
+// This thread spins forever until signaled to stop.
+class BusyWaitingThread {
+ public:
+  BusyWaitingThread() : m_run(1), m_success(0) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(21);
+    system_clock::time_point start_time = system_clock::now();
+    m_started.Notify();
+    while (m_run.test_and_set()) {
+      // Have the busy waiting thread time out after a while.  If it times out,
+      // the test failed.
+      if (system_clock::now() - start_time > std::chrono::milliseconds(50)) {
+        return;
+      }
+    }
+    m_success.store(1);
+  }
+
+  void quit() { m_run.clear(); }
+  void WaitForStartup() { m_started.Wait(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  // Use an atomic type to signal if the thread should be running or not.  A
+  // mutex could affect the scheduler, which isn't worth it.  A delay between
+  // setting and reading the new value is fine.
+  std::atomic_flag m_run;
+
+  Notification m_started;
+
+  std::atomic<int> m_success;
+};
+
+// This thread starts up, grabs the mutex, and then exits.
+template <typename MutexType>
+class HighPriorityThread {
+ public:
+  HighPriorityThread(MutexType *mutex) : m_mutex(mutex), m_success(0) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(22);
+    m_started.Notify();
+    m_mutex->lock();
+    m_success.store(1);
+  }
+
+  void WaitForStartup() { m_started.Wait(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  Notification m_started;
+  MutexType *m_mutex;
+  std::atomic<int> m_success;
+};
+
+// Class to test a MutexType to see if it solves the priority inheritance
+// problem.
+//
+// To run the test, we need 3 threads, and then 1 thread to kick the test off.
+// The threads must all run on the same core, otherwise they wouldn't starve
+// eachother. The threads and their roles are as follows:
+//
+// Low priority thread:
+//   Holds a lock that the high priority thread needs, and releases it upon
+//   request.
+// Medium priority thread:
+//   Hogs the processor so that the low priority thread will never run if it's
+//   priority doesn't get bumped.
+// High priority thread:
+//   Starts up and then goes to grab the lock that the low priority thread has.
+//
+// Control thread:
+//   Sets the deadlock up so that it will happen 100% of the time by making sure
+//   that each thread in order gets to the point that it needs to be at to cause
+//   the deadlock.
+template <typename MutexType>
+class InversionTestRunner {
+ public:
+  void operator()() {
+    // This thread must run at the highest priority or it can't coordinate the
+    // inversion.  This means that it can't busy wait or everything could
+    // starve.
+    SetThreadRealtimePriorityOrDie(23);
+
+    MutexType m;
+
+    // Start the lowest priority thread and wait until it holds the lock.
+    LowPriorityThread<MutexType> low(&m);
+    std::thread low_thread(std::ref(low));
+    low.WaitForStartup();
+
+    // Start the busy waiting thread and let it get to the loop.
+    BusyWaitingThread busy;
+    std::thread busy_thread(std::ref(busy));
+    busy.WaitForStartup();
+
+    // Start the high priority thread and let it become blocked on the lock.
+    HighPriorityThread<MutexType> high(&m);
+    std::thread high_thread(std::ref(high));
+    high.WaitForStartup();
+    // Startup and locking the mutex in the high priority thread aren't atomic,
+    // but pretty close.  Wait a bit to let it happen.
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+    // Release the mutex in the low priority thread.  If done right, everything
+    // should finish now.
+    low.release_mutex();
+
+    // Wait for everything to finish and compute success.
+    high_thread.join();
+    busy.quit();
+    busy_thread.join();
+    low_thread.join();
+    m_success = low.success() && busy.success() && high.success();
+  }
+
+  bool success() { return m_success; }
+
+ private:
+  bool m_success = false;
+};
+
+//TODO: Fix roborio permissions to run as root.
+
+// Priority inversion test.
+TEST(MutexTest, DISABLED_PriorityInversionTest) {
+  InversionTestRunner<priority_mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_TRUE(runner.success());
+}
+
+// Verify that the non-priority inversion mutex doesn't pass the test.
+TEST(MutexTest, DISABLED_StdMutexPriorityInversionTest) {
+  InversionTestRunner<std::mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_FALSE(runner.success());
+}
+
+// Smoke test to make sure that mutexes lock and unlock.
+TEST(MutexTest, TryLock) {
+  priority_mutex m;
+  m.lock();
+  EXPECT_FALSE(m.try_lock());
+  m.unlock();
+  EXPECT_TRUE(m.try_lock());
+}
+
+// Priority inversion test.
+TEST(MutexTest, DISABLED_ReentrantPriorityInversionTest) {
+  InversionTestRunner<priority_recursive_mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_TRUE(runner.success());
+}
+
+// Smoke test to make sure that mutexes lock and unlock.
+TEST(MutexTest, ReentrantTryLock) {
+  priority_recursive_mutex m;
+  m.lock();
+  EXPECT_TRUE(m.try_lock());
+  m.unlock();
+  EXPECT_TRUE(m.try_lock());
+}
+
+}  // namespace testing
+}  // namespace wpilib
diff --git a/wpilibcIntegrationTests/src/NotifierTest.cpp b/wpilibcIntegrationTests/src/NotifierTest.cpp
new file mode 100644
index 0000000..10e0504
--- /dev/null
+++ b/wpilibcIntegrationTests/src/NotifierTest.cpp
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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>
+#include <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+unsigned notifierCounter;
+
+void notifierHandler(void *) { notifierCounter++; }
+
+/**
+ * Test if the Wait function works
+ */
+TEST(NotifierTest, DISABLED_TestTimerNotifications) {
+  std::cout << "NotifierTest..." << std::endl;
+  notifierCounter = 0;
+  std::cout << "notifier(notifierHandler, nullptr)..." << std::endl;
+  Notifier notifier(notifierHandler, nullptr);
+  std::cout << "Start Periodic..." << std::endl;
+  notifier.StartPeriodic(1.0);
+
+  std::cout << "Wait..." << std::endl;
+  Wait(10.5);
+  std::cout << "...Wait" << std::endl;
+
+  EXPECT_EQ(10u, notifierCounter) << "Received " << notifierCounter
+                                  << " notifications in 10.5 seconds";
+  std::cout << "Received " << notifierCounter
+            << " notifications in 10.5 seconds";
+
+  std::cout << "...NotifierTest" << std::endl;
+}
diff --git a/wpilibcIntegrationTests/src/PCMTest.cpp b/wpilibcIntegrationTests/src/PCMTest.cpp
new file mode 100644
index 0000000..14f23ca
--- /dev/null
+++ b/wpilibcIntegrationTests/src/PCMTest.cpp
@@ -0,0 +1,154 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+/* 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;
+
+  virtual 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);
+  }
+
+  virtual 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.1)
+      << "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.1)
+      << "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";
+}
diff --git a/wpilibcIntegrationTests/src/PowerDistributionPanelTest.cpp b/wpilibcIntegrationTests/src/PowerDistributionPanelTest.cpp
new file mode 100644
index 0000000..7dc31fd
--- /dev/null
+++ b/wpilibcIntegrationTests/src/PowerDistributionPanelTest.cpp
@@ -0,0 +1,95 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <Jaguar.h>
+#include <PowerDistributionPanel.h>
+#include <Talon.h>
+#include <Timer.h>
+#include <Victor.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+static const double kMotorTime = 0.25;
+
+class PowerDistributionPanelTest : public testing::Test {
+ protected:
+  PowerDistributionPanel *m_pdp;
+  Talon *m_talon;
+  Victor *m_victor;
+  Jaguar *m_jaguar;
+
+  virtual 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);
+  }
+
+  virtual 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";
+}
+
+/**
+ * Test if the current changes when the motor is driven using a victor
+ */
+TEST_F(PowerDistributionPanelTest, CheckCurrentVictor) {
+  Wait(kMotorTime);
+
+  /* The Current should be 0 */
+  EXPECT_FLOAT_EQ(0, m_pdp->GetCurrent(TestBench::kVictorPDPChannel))
+      << "The Victor current was non-zero";
+
+  /* Set the motor to full forward */
+  m_victor->Set(1.0);
+  Wait(kMotorTime);
+
+  /* The current should now be positive */
+  ASSERT_GT(m_pdp->GetCurrent(TestBench::kVictorPDPChannel), 0)
+      << "The Victor current was not positive";
+}
+
+/**
+ * Test if the current changes when the motor is driven using a jaguar
+ */
+TEST_F(PowerDistributionPanelTest, CheckCurrentJaguar) {
+  Wait(kMotorTime);
+
+  /* The Current should be 0 */
+  EXPECT_FLOAT_EQ(0, m_pdp->GetCurrent(TestBench::kJaguarPDPChannel))
+      << "The Jaguar current was non-zero";
+
+  /* Set the motor to full forward */
+  m_jaguar->Set(1.0);
+  Wait(kMotorTime);
+
+  /* The current should now be positive */
+  ASSERT_GT(m_pdp->GetCurrent(TestBench::kJaguarPDPChannel), 0)
+      << "The Jaguar current was not positive";
+}
diff --git a/wpilibcIntegrationTests/src/PreferencesTest.cpp b/wpilibcIntegrationTests/src/PreferencesTest.cpp
new file mode 100644
index 0000000..030d373
--- /dev/null
+++ b/wpilibcIntegrationTests/src/PreferencesTest.cpp
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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>
+#include <Timer.h>
+#include "gtest/gtest.h"
+#include <cstdio>
+#include <fstream>
+
+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) {
+  NetworkTable::Shutdown();
+  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();
+  NetworkTable::Initialize();
+
+  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) {
+  NetworkTable::Shutdown();
+  NetworkTable::GlobalDeleteAll();
+  std::remove(kFileName);
+  NetworkTable::Initialize();
+  Preferences *preferences = Preferences::GetInstance();
+  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);
+  preferences->Save();
+
+  Wait(kSaveTime);
+
+  static char const *kExpectedFileContents[] = {
+      "[NetworkTables Storage 3.0]",
+      "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";
+  }
+}
diff --git a/wpilibcIntegrationTests/src/RelayTest.cpp b/wpilibcIntegrationTests/src/RelayTest.cpp
new file mode 100644
index 0000000..c919297
--- /dev/null
+++ b/wpilibcIntegrationTests/src/RelayTest.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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>
+#include <Relay.h>
+#include <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+#include "Relay.h"
+
+static const double kDelayTime = 0.01;
+
+class RelayTest : public testing::Test {
+ protected:
+  Relay *m_relay;
+  DigitalInput *m_forward;
+  DigitalInput *m_reverse;
+
+  virtual void SetUp() override {
+    m_relay = new Relay(TestBench::kRelayChannel);
+    m_forward = new DigitalInput(TestBench::kFakeRelayForward);
+    m_reverse = new DigitalInput(TestBench::kFakeRelayReverse);
+  }
+
+  virtual void TearDown() override {
+    delete m_relay;
+    delete m_forward;
+    delete m_reverse;
+  }
+
+  virtual 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";
+
+  // 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";
+
+  // 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";
+
+  // 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";
+}
diff --git a/wpilibcIntegrationTests/src/TestEnvironment.cpp b/wpilibcIntegrationTests/src/TestEnvironment.cpp
new file mode 100644
index 0000000..d65493a
--- /dev/null
+++ b/wpilibcIntegrationTests/src/TestEnvironment.cpp
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <LiveWindow/LiveWindow.h>
+#include <Timer.h>
+#include "gtest/gtest.h"
+
+class TestEnvironment : public testing::Environment {
+  bool m_alreadySetUp = false;
+
+ public:
+  virtual 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 (!HALInitialize()) {
+      std::cerr << "FATAL ERROR: HAL could not be initialized" << std::endl;
+      exit(-1);
+    }
+
+    /* 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. */
+    HALNetworkCommunicationObserveUserProgramStarting();
+    LiveWindow::GetInstance()->SetEnabled(false);
+
+    std::cout << "Waiting for enable" << std::endl;
+
+    while (!DriverStation::GetInstance().IsEnabled()) {
+      Wait(0.1);
+    }
+  }
+
+  virtual void TearDown() override {}
+};
+
+testing::Environment *const environment =
+    testing::AddGlobalTestEnvironment(new TestEnvironment);
diff --git a/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp b/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp
new file mode 100644
index 0000000..35b8d29
--- /dev/null
+++ b/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp
@@ -0,0 +1,169 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 <ADXL345_SPI.h>
+#include <AnalogGyro.h>
+#include <Servo.h>
+#include <Timer.h>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+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
+ * @author Thomas Clark
+ */
+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; }
+
+  virtual void SetUp() override {
+    m_tilt = new Servo(TestBench::kCameraTiltChannel);
+    m_pan = new Servo(TestBench::kCameraPanChannel);
+    m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS0);
+
+    m_tilt->Set(kTiltSetpoint45);
+    m_pan->SetAngle(0.0f);
+
+    Wait(kServoResetTime);
+
+    m_gyro->Reset();
+  }
+
+  void DefaultGyroAngle();
+  void GyroAngle();
+  void GyroCalibratedParameters();
+
+  virtual 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.0f, m_gyro->GetAngle(), 1.0f);
+}
+
+/**
+ * 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 (int 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();
+  float 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(.001);
+  EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f);
+
+  // 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 (int 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);
+}
diff --git a/wpilibcIntegrationTests/src/TimerTest.cpp b/wpilibcIntegrationTests/src/TimerTest.cpp
new file mode 100644
index 0000000..faa6ada
--- /dev/null
+++ b/wpilibcIntegrationTests/src/TimerTest.cpp
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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>
+#include "gtest/gtest.h"
+#include "TestBench.h"
+
+static const double kWaitTime = 0.5;
+
+class TimerTest : public testing::Test {
+ protected:
+  Timer *m_timer;
+
+  virtual void SetUp() override { m_timer = new Timer; }
+
+  virtual 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);
+}
diff --git a/wpilibcIntegrationTests/src/command/CommandTest.cpp b/wpilibcIntegrationTests/src/command/CommandTest.cpp
new file mode 100644
index 0000000..3e16021
--- /dev/null
+++ b/wpilibcIntegrationTests/src/command/CommandTest.cpp
@@ -0,0 +1,441 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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"
+
+class CommandTest : public testing::Test {
+ protected:
+  virtual 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, int initialize, int execute,
+                          int isFinished, int end, int 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:
+  ASubsystem(const std::string &name) : Subsystem(name) {}
+
+  virtual 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();
+}
diff --git a/wpilibcIntegrationTests/src/command/MockCommand.cpp b/wpilibcIntegrationTests/src/command/MockCommand.cpp
new file mode 100644
index 0000000..a4d22f8
--- /dev/null
+++ b/wpilibcIntegrationTests/src/command/MockCommand.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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"
+
+MockCommand::MockCommand() {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_hasFinished = false;
+  m_endCount = 0;
+  m_interruptedCount = 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; }
+
+bool MockCommand::HasInitialized() { return GetInitializeCount() > 0; }
+
+bool MockCommand::HasEnd() { return GetEndCount() > 0; }
+
+bool MockCommand::HasInterrupted() { return GetInterruptedCount() > 0; }