diff --git a/wpilibc/sim/src/AnalogGyro.cpp b/wpilibc/sim/src/AnalogGyro.cpp
new file mode 100644
index 0000000..77b231c
--- /dev/null
+++ b/wpilibc/sim/src/AnalogGyro.cpp
@@ -0,0 +1,83 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "AnalogGyro.h"
+
+#include <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const int AnalogGyro::kOversampleBits = 10;
+const int AnalogGyro::kAverageBits = 0;
+const double AnalogGyro::kSamplesPerSecond = 50.0;
+const double AnalogGyro::kCalibrationSampleTime = 5.0;
+const double AnalogGyro::kDefaultVoltsPerDegreePerSecond = 0.007;
+
+/**
+ * Initialize the gyro.
+ *
+ * Calibrate the gyro by running for a number of samples and computing the
+ * center value for this part. Then use the center value as the Accumulator
+ * center value for subsequent measurements. It's important to make sure that
+ * the robot is not moving while the centering calculations are in progress,
+ * this is typically done when the robot is first turned on while it's sitting
+ * at rest before the competition starts.
+ */
+void AnalogGyro::InitAnalogGyro(int channel) {
+  SetPIDSourceType(PIDSourceType::kDisplacement);
+
+  std::stringstream ss;
+  ss << "analog/" << channel;
+  impl = new SimGyro(ss.str());
+
+  LiveWindow::GetInstance()->AddSensor("AnalogGyro", channel, this);
+}
+
+/**
+ * AnalogGyro constructor with only a channel.
+ *
+ * @param channel The analog channel the gyro is connected to.
+ */
+AnalogGyro::AnalogGyro(int channel) { InitAnalogGyro(channel); }
+
+/**
+ * Reset the gyro.
+ *
+ * Resets the gyro to a heading of zero. This can be used if there is
+ * significant drift in the gyro and it needs to be recalibrated after it has
+ * been running.
+ */
+void AnalogGyro::Reset() { impl->Reset(); }
+
+void AnalogGyro::Calibrate() { Reset(); }
+
+/**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * The angle is based on the current accumulator value corrected by the
+ * oversampling rate, the gyro type and the A/D calibration values. The angle
+ * is continuous, that is can go beyond 360 degrees. This make algorithms that
+ * wouldn't want to see a discontinuity in the gyro output as it sweeps past 0
+ * on the second time around.
+ *
+ * @return the current heading of the robot in degrees. This heading is based on
+ *         integration of the returned rate from the gyro.
+ */
+double AnalogGyro::GetAngle() const { return impl->GetAngle(); }
+
+/**
+ * Return the rate of rotation of the gyro
+ *
+ * The rate is based on the most recent reading of the gyro analog value
+ *
+ * @return the current rate in degrees per second
+ */
+double AnalogGyro::GetRate() const { return impl->GetVelocity(); }
diff --git a/wpilibc/sim/src/AnalogInput.cpp b/wpilibc/sim/src/AnalogInput.cpp
new file mode 100644
index 0000000..0c00b6e
--- /dev/null
+++ b/wpilibc/sim/src/AnalogInput.cpp
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Construct an analog input.
+ *
+ * @param channel The channel number to represent.
+ */
+AnalogInput::AnalogInput(int channel) : m_channel(channel) {
+  std::stringstream ss;
+  ss << "analog/" << channel;
+  m_impl = new SimFloatInput(ss.str());
+
+  LiveWindow::GetInstance()->AddSensor("AnalogInput", channel, this);
+}
+
+/**
+ * Get a scaled sample straight from this channel.
+ *
+ * The value is scaled to units of Volts using the calibrated scaling data from
+ * GetLSBWeight() and GetOffset().
+ *
+ * @return A scaled sample straight from this channel.
+ */
+double AnalogInput::GetVoltage() const { return m_impl->Get(); }
+
+/**
+ * Get a scaled sample from the output of the oversample and average engine for
+ * this channel.
+ *
+ * The value is scaled to units of Volts using the calibrated scaling data from
+ * GetLSBWeight() and GetOffset(). Using oversampling will cause this value to
+ * be higher resolution, but it will update more slowly. Using averaging will
+ * cause this value to be more stable, but it will update more slowly.
+ *
+ * @return A scaled sample from the output of the oversample and average engine
+ *         for this channel.
+ */
+double AnalogInput::GetAverageVoltage() const { return m_impl->Get(); }
+
+/**
+ * Get the channel number.
+ *
+ * @return The channel number.
+ */
+int AnalogInput::GetChannel() const { return m_channel; }
+
+/**
+ * Get the Average value for the PID Source base object.
+ *
+ * @return The average voltage.
+ */
+double AnalogInput::PIDGet() { return GetAverageVoltage(); }
+
+void AnalogInput::UpdateTable() {
+  if (m_table != nullptr) {
+    m_table->PutNumber("Value", GetAverageVoltage());
+  }
+}
+
+void AnalogInput::StartLiveWindowMode() {}
+
+void AnalogInput::StopLiveWindowMode() {}
+
+std::string AnalogInput::GetSmartDashboardType() const {
+  return "Analog Input";
+}
+
+void AnalogInput::InitTable(std::shared_ptr<ITable> subTable) {
+  m_table = subTable;
+  UpdateTable();
+}
+
+std::shared_ptr<ITable> AnalogInput::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/AnalogPotentiometer.cpp b/wpilibc/sim/src/AnalogPotentiometer.cpp
new file mode 100644
index 0000000..dcffea6
--- /dev/null
+++ b/wpilibc/sim/src/AnalogPotentiometer.cpp
@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogPotentiometer.h"
+
+using namespace frc;
+
+/**
+ * Common initialization code called by all constructors.
+ */
+void AnalogPotentiometer::initPot(AnalogInput* input, double scale,
+                                  double offset) {
+  m_scale = scale;
+  m_offset = offset;
+  m_analog_input = input;
+}
+
+AnalogPotentiometer::AnalogPotentiometer(int channel, double scale,
+                                         double offset) {
+  m_init_analog_input = true;
+  initPot(new AnalogInput(channel), scale, offset);
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double scale,
+                                         double offset) {
+  m_init_analog_input = false;
+  initPot(input, scale, offset);
+}
+
+AnalogPotentiometer::AnalogPotentiometer(AnalogInput& input, double scale,
+                                         double offset) {
+  m_init_analog_input = false;
+  initPot(&input, scale, offset);
+}
+
+AnalogPotentiometer::~AnalogPotentiometer() {
+  if (m_init_analog_input) {
+    delete m_analog_input;
+    m_init_analog_input = false;
+  }
+}
+
+/**
+ * Get the current reading of the potentiomere.
+ *
+ * @return The current position of the potentiometer.
+ */
+double AnalogPotentiometer::Get() const {
+  return m_analog_input->GetVoltage() * m_scale + m_offset;
+}
+
+/**
+ * Implement the PIDSource interface.
+ *
+ * @return The current reading.
+ */
+double AnalogPotentiometer::PIDGet() { return Get(); }
+
+/**
+ * @return the Smart Dashboard Type
+ */
+std::string AnalogPotentiometer::GetSmartDashboardType() const {
+  return "Analog Input";
+}
+
+/**
+ * Live Window code, only does anything if live window is activated.
+ */
+void AnalogPotentiometer::InitTable(std::shared_ptr<ITable> subtable) {
+  m_table = subtable;
+  UpdateTable();
+}
+
+void AnalogPotentiometer::UpdateTable() {
+  if (m_table != nullptr) {
+    m_table->PutNumber("Value", Get());
+  }
+}
+
+std::shared_ptr<ITable> AnalogPotentiometer::GetTable() const {
+  return m_table;
+}
diff --git a/wpilibc/sim/src/DigitalInput.cpp b/wpilibc/sim/src/DigitalInput.cpp
new file mode 100644
index 0000000..a560761
--- /dev/null
+++ b/wpilibc/sim/src/DigitalInput.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 <sstream>
+
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Create an instance of a Digital Input class.
+ * Creates a digital input given a channel and uses the default module.
+ *
+ * @param channel The digital channel (1..14).
+ */
+DigitalInput::DigitalInput(int channel) : m_channel(channel) {
+  std::stringstream ss;
+  ss << "dio/" << channel;
+  m_impl = new SimDigitalInput(ss.str());
+}
+
+/**
+ * Get the value from a digital input channel.
+ * Retrieve the value of a single digital input channel from the FPGA.
+ */
+int DigitalInput::Get() const { return m_impl->Get(); }
+
+/**
+ * @return The GPIO channel number that this object represents.
+ */
+int DigitalInput::GetChannel() const { return m_channel; }
+
+void DigitalInput::UpdateTable() {
+  if (m_table != nullptr) {
+    m_table->PutBoolean("Value", Get());
+  }
+}
+
+void DigitalInput::StartLiveWindowMode() {}
+
+void DigitalInput::StopLiveWindowMode() {}
+
+std::string DigitalInput::GetSmartDashboardType() const {
+  return "DigitalInput";
+}
+
+void DigitalInput::InitTable(std::shared_ptr<ITable> subTable) {
+  m_table = subTable;
+  UpdateTable();
+}
+
+std::shared_ptr<ITable> DigitalInput::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/DoubleSolenoid.cpp b/wpilibc/sim/src/DoubleSolenoid.cpp
new file mode 100644
index 0000000..1607dc8
--- /dev/null
+++ b/wpilibc/sim/src/DoubleSolenoid.cpp
@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "DoubleSolenoid.h"
+
+#include "LiveWindow/LiveWindow.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Constructor.
+ *
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(int forwardChannel, int reverseChannel)
+    : DoubleSolenoid(1, forwardChannel, reverseChannel) {}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber   The solenoid module (1 or 2).
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(int moduleNumber, int forwardChannel,
+                               int reverseChannel) {
+  m_reversed = false;
+  if (reverseChannel < forwardChannel) {  // Swap ports to get the right address
+    int channel = reverseChannel;
+    reverseChannel = forwardChannel;
+    forwardChannel = channel;
+    m_reversed = true;
+  }
+  std::stringstream ss;
+  ss << "pneumatic/" << moduleNumber << "/" << forwardChannel << "/"
+     << moduleNumber << "/" << reverseChannel;
+  m_impl = new SimContinuousOutput(ss.str());
+
+  LiveWindow::GetInstance()->AddActuator("DoubleSolenoid", moduleNumber,
+                                         forwardChannel, this);
+}
+
+DoubleSolenoid::~DoubleSolenoid() {
+  if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param value Move the solenoid to forward, reverse, or don't move it.
+ */
+void DoubleSolenoid::Set(Value value) {
+  m_value = value;
+  switch (value) {
+    case kOff:
+      m_impl->Set(0);
+      break;
+    case kForward:
+      m_impl->Set(m_reversed ? -1 : 1);
+      break;
+    case kReverse:
+      m_impl->Set(m_reversed ? 1 : -1);
+      break;
+  }
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+DoubleSolenoid::Value DoubleSolenoid::Get() const { return m_value; }
+
+void DoubleSolenoid::ValueChanged(ITable* source, llvm::StringRef key,
+                                  std::shared_ptr<nt::Value> value,
+                                  bool isNew) {
+  if (!value->IsString()) return;
+  Value lvalue = kOff;
+  if (value->GetString() == "Forward")
+    lvalue = kForward;
+  else if (value->GetString() == "Reverse")
+    lvalue = kReverse;
+  Set(lvalue);
+}
+
+void DoubleSolenoid::UpdateTable() {
+  if (m_table != nullptr) {
+    m_table->PutString(
+        "Value", (Get() == kForward ? "Forward"
+                                    : (Get() == kReverse ? "Reverse" : "Off")));
+  }
+}
+
+void DoubleSolenoid::StartLiveWindowMode() {
+  Set(kOff);
+  if (m_table != nullptr) {
+    m_table->AddTableListener("Value", this, true);
+  }
+}
+
+void DoubleSolenoid::StopLiveWindowMode() {
+  Set(kOff);
+  if (m_table != nullptr) {
+    m_table->RemoveTableListener(this);
+  }
+}
+
+std::string DoubleSolenoid::GetSmartDashboardType() const {
+  return "Double Solenoid";
+}
+
+void DoubleSolenoid::InitTable(std::shared_ptr<ITable> subTable) {
+  m_table = subTable;
+  UpdateTable();
+}
+
+std::shared_ptr<ITable> DoubleSolenoid::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/DriverStation.cpp b/wpilibc/sim/src/DriverStation.cpp
new file mode 100644
index 0000000..1e43556
--- /dev/null
+++ b/wpilibc/sim/src/DriverStation.cpp
@@ -0,0 +1,410 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 <boost/mem_fn.hpp>
+
+#include "HAL/cpp/Log.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+const int DriverStation::kBatteryChannel;
+const int DriverStation::kJoystickPorts;
+const int DriverStation::kJoystickAxes;
+const double DriverStation::kUpdatePeriod = 0.02;
+int DriverStation::m_updateNumber = 0;
+
+/**
+ * DriverStation contructor.
+ *
+ * This is only called once the first time GetInstance() is called
+ */
+DriverStation::DriverStation() {
+  state = gazebo::msgs::DriverStationPtr(new gazebo::msgs::DriverStation());
+  stateSub =
+      MainNode::Subscribe("~/ds/state", &DriverStation::stateCallback, this);
+  // TODO: for loop + boost bind
+  joysticks[0] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+  joysticksSub[0] = MainNode::Subscribe(
+      "~/ds/joysticks/0", &DriverStation::joystickCallback0, this);
+  joysticks[1] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+  joysticksSub[1] = MainNode::Subscribe(
+      "~/ds/joysticks/1", &DriverStation::joystickCallback1, this);
+  joysticks[2] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+  joysticksSub[2] = MainNode::Subscribe(
+      "~/ds/joysticks/2", &DriverStation::joystickCallback2, this);
+  joysticks[3] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+  joysticksSub[3] = MainNode::Subscribe(
+      "~/ds/joysticks/5", &DriverStation::joystickCallback3, this);
+  joysticks[4] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+  joysticksSub[4] = MainNode::Subscribe(
+      "~/ds/joysticks/4", &DriverStation::joystickCallback4, this);
+  joysticks[5] = gazebo::msgs::FRCJoystickPtr(new gazebo::msgs::FRCJoystick());
+  joysticksSub[5] = MainNode::Subscribe(
+      "~/ds/joysticks/5", &DriverStation::joystickCallback5, this);
+}
+
+/**
+ * Return a pointer to the singleton DriverStation.
+ */
+DriverStation& DriverStation::GetInstance() {
+  static DriverStation instance;
+  return instance;
+}
+
+/**
+ * Read the battery voltage. Hardcoded to 12 volts for Simulation.
+ *
+ * @return The battery voltage.
+ */
+double DriverStation::GetBatteryVoltage() const {
+  return 12.0;  // 12 volts all the time!
+}
+
+/**
+ * Get the value of the axis on a joystick.
+ * This depends on the mapping of the joystick connected to the specified port.
+ *
+ * @param stick The joystick to read.
+ * @param axis  The analog axis value to read from the joystick.
+ * @return The value of the axis on the joystick.
+ */
+double DriverStation::GetStickAxis(int stick, int axis) {
+  if (axis < 0 || axis > (kJoystickAxes - 1)) {
+    wpi_setWPIError(BadJoystickAxis);
+    return 0.0;
+  }
+  if (stick < 0 || stick > 5) {
+    wpi_setWPIError(BadJoystickIndex);
+    return 0.0;
+  }
+
+  std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+  if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size()) {
+    return 0.0;
+  }
+  return joysticks[stick]->axes(axis);
+}
+
+/**
+ * The state of a specific button (1 - 12) on the joystick.
+ *
+ * This method only works in simulation, but is more efficient than
+ * GetStickButtons.
+ *
+ * @param stick  The joystick to read.
+ * @param button The button number to check.
+ * @return If the button is pressed.
+ */
+bool DriverStation::GetStickButton(int stick, int button) {
+  if (stick < 0 || stick >= 6) {
+    wpi_setWPIErrorWithContext(ParameterOutOfRange,
+                               "stick must be between 0 and 5");
+    return false;
+  }
+
+  std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+  if (joysticks[stick] == nullptr ||
+      button >= joysticks[stick]->buttons().size()) {
+    return false;
+  }
+  return joysticks[stick]->buttons(button - 1);
+}
+
+/**
+ * The state of the buttons on the joystick.
+ *
+ * 12 buttons (4 msb are unused) from the joystick.
+ *
+ * @param stick The joystick to read.
+ * @return The state of the buttons on the joystick.
+ */
+int16_t DriverStation::GetStickButtons(int stick) {
+  if (stick < 0 || stick >= 6) {
+    wpi_setWPIErrorWithContext(ParameterOutOfRange,
+                               "stick must be between 0 and 5");
+    return false;
+  }
+  int16_t btns = 0, btnid;
+
+  std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+  gazebo::msgs::FRCJoystickPtr joy = joysticks[stick];
+  for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++) {
+    if (joysticks[stick]->buttons(btnid)) {
+      btns |= (1 << btnid);
+    }
+  }
+  return btns;
+}
+
+// 5V divided by 10 bits
+#define kDSAnalogInScaling (5.0 / 1023.0)
+
+/**
+ * Get an analog voltage from the Driver Station.
+ *
+ * The analog values are returned as voltage values for the Driver Station
+ * analog inputs. These inputs are typically used for advanced operator
+ * interfaces consisting of potentiometers or resistor networks representing
+ * values on a rotary switch.
+ *
+ * @param channel The analog input channel on the driver station to read from.
+ *                Valid range is 1 - 4.
+ * @return The analog voltage on the input.
+ */
+double DriverStation::GetAnalogIn(int channel) {
+  wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn");
+  return 0.0;
+}
+
+/**
+ * Get values from the digital inputs on the Driver Station.
+ *
+ * Return digital values from the Drivers Station. These values are typically
+ * used for buttons and switches on advanced operator interfaces.
+ *
+ * @param channel The digital input to get. Valid range is 1 - 8.
+ */
+bool DriverStation::GetDigitalIn(int channel) {
+  wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn");
+  return false;
+}
+
+/**
+ * Set a value for the digital outputs on the Driver Station.
+ *
+ * Control digital outputs on the Drivers Station. These values are typically
+ * used for giving feedback on a custom operator station such as LEDs.
+ *
+ * @param channel The digital output to set. Valid range is 1 - 8.
+ * @param value   The state to set the digital output.
+ */
+void DriverStation::SetDigitalOut(int channel, bool value) {
+  wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut");
+}
+
+/**
+ * Get a value that was set for the digital outputs on the Driver Station.
+ *
+ * @param channel The digital ouput to monitor. Valid range is 1 through 8.
+ * @return A digital value being output on the Drivers Station.
+ */
+bool DriverStation::GetDigitalOut(int channel) {
+  wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut");
+  return false;
+}
+
+bool DriverStation::IsEnabled() const {
+  std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+  return state != nullptr ? state->enabled() : false;
+}
+
+bool DriverStation::IsDisabled() const { return !IsEnabled(); }
+
+bool DriverStation::IsAutonomous() const {
+  std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+  return state != nullptr
+             ? state->state() == gazebo::msgs::DriverStation_State_AUTO
+             : false;
+}
+
+bool DriverStation::IsOperatorControl() const {
+  return !(IsAutonomous() || IsTest());
+}
+
+bool DriverStation::IsTest() const {
+  std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+  return state != nullptr
+             ? state->state() == gazebo::msgs::DriverStation_State_TEST
+             : false;
+}
+
+/**
+ * Is the driver station attached to a Field Management System?
+ * @return True if the robot is competing on a field being controlled by a Field
+ *         Management System
+ */
+bool DriverStation::IsFMSAttached() const {
+  return false;  // No FMS in simulation
+}
+
+/**
+ * Return the alliance that the driver station says it is on.
+ * This could return kRed or kBlue.
+ * @return The Alliance enum
+ */
+DriverStation::Alliance DriverStation::GetAlliance() const {
+  // if (m_controlData->dsID_Alliance == 'R') return kRed;
+  // if (m_controlData->dsID_Alliance == 'B') return kBlue;
+  // wpi_assert(false);
+  return kInvalid;  // TODO: Support alliance colors
+}
+
+/**
+ * Return the driver station location on the field.
+ * This could return 1, 2, or 3.
+ * @return The location of the driver station
+ */
+int DriverStation::GetLocation() const {
+  return -1;  // TODO: Support locations
+}
+
+/**
+ * Wait until a new packet comes from the driver station.
+ *
+ * This blocks on a semaphore, so the waiting is efficient.
+ *
+ * This is a good way to delay processing until there is new driver station data
+ * to act on.
+ */
+void DriverStation::WaitForData() { WaitForData(0); }
+
+/**
+ * Wait until a new packet comes from the driver station, or wait for a timeout.
+ *
+ * If the timeout is less then or equal to 0, wait indefinitely.
+ *
+ * Timeout is in milliseconds
+ *
+ * This blocks on a semaphore, so the waiting is efficient.
+ *
+ * This is a good way to delay processing until there is new driver station data
+ * to act on.
+ *
+ * @param timeout Timeout time in seconds
+ *
+ * @return true if new data, otherwise false
+ */
+bool DriverStation::WaitForData(double timeout) {
+#if defined(_MSC_VER) && _MSC_VER < 1900
+  auto timeoutTime = std::chrono::steady_clock::now() +
+                     std::chrono::duration<int64_t, std::nano>(
+                         static_cast<int64_t>(timeout * 1e9));
+#else
+  auto timeoutTime =
+      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+#endif
+
+  std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
+  while (!m_updatedControlLoopData) {
+    if (timeout > 0) {
+      auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
+      if (timedOut == std::cv_status::timeout) {
+        return false;
+      }
+    } else {
+      m_waitForDataCond.wait(lock);
+    }
+  }
+  m_updatedControlLoopData = false;
+  return true;
+}
+
+/**
+ * Return the approximate match time.
+ * The FMS does not currently send the official match time to the robots
+ * This returns the time since the enable signal sent from the Driver Station
+ * At the beginning of autonomous, the time is reset to 0.0 seconds
+ * At the beginning of teleop, the time is reset to +15.0 seconds
+ * If the robot is disabled, this returns 0.0 seconds
+ * Warning: This is not an official time (so it cannot be used to argue with
+ * referees)
+ * @return Match time in seconds since the beginning of autonomous
+ */
+double DriverStation::GetMatchTime() const {
+  if (m_approxMatchTimeOffset < 0.0) return 0.0;
+  return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(llvm::StringRef error) {
+  std::cout << error << std::endl;
+}
+
+/**
+ * Report a warning to the DriverStation messages window.
+ * The warning is also printed to the program console.
+ */
+void DriverStation::ReportWarning(llvm::StringRef error) {
+  std::cout << error << std::endl;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(bool is_error, int code, llvm::StringRef error,
+                                llvm::StringRef location,
+                                llvm::StringRef stack) {
+  if (!location.empty())
+    std::cout << (is_error ? "Error" : "Warning") << " at " << location << ": ";
+  std::cout << error << std::endl;
+  if (!stack.empty()) std::cout << stack << std::endl;
+}
+
+/**
+ * Return the team number that the Driver Station is configured for.
+ * @return The team number
+ */
+uint16_t DriverStation::GetTeamNumber() const { return 348; }
+
+void DriverStation::stateCallback(
+    const gazebo::msgs::ConstDriverStationPtr& msg) {
+  {
+    std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+    *state = *msg;
+  }
+  {
+    std::lock_guard<priority_mutex> lock(m_waitForDataMutex);
+    m_updatedControlLoopData = true;
+  }
+  m_waitForDataCond.notify_all();
+}
+
+void DriverStation::joystickCallback(
+    const gazebo::msgs::ConstFRCJoystickPtr& msg, int i) {
+  std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+  *(joysticks[i]) = *msg;
+}
+
+void DriverStation::joystickCallback0(
+    const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+  joystickCallback(msg, 0);
+}
+
+void DriverStation::joystickCallback1(
+    const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+  joystickCallback(msg, 1);
+}
+
+void DriverStation::joystickCallback2(
+    const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+  joystickCallback(msg, 2);
+}
+
+void DriverStation::joystickCallback3(
+    const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+  joystickCallback(msg, 3);
+}
+
+void DriverStation::joystickCallback4(
+    const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+  joystickCallback(msg, 4);
+}
+
+void DriverStation::joystickCallback5(
+    const gazebo::msgs::ConstFRCJoystickPtr& msg) {
+  joystickCallback(msg, 5);
+}
diff --git a/wpilibc/sim/src/Encoder.cpp b/wpilibc/sim/src/Encoder.cpp
new file mode 100644
index 0000000..cef4bef
--- /dev/null
+++ b/wpilibc/sim/src/Encoder.cpp
@@ -0,0 +1,388 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Common initialization code for Encoders.
+ * This code allocates resources for Encoders and is common to all constructors.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param reverseDirection If true, counts down instead of up (this is all
+ *                         relative)
+ * @param encodingType     either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ *                         decoding. If 4X is selected, then an encoder FPGA
+ *                         object is used and the returned counts will be 4x
+ *                         the encoder spec'd value since all rising and
+ *                         falling edges are counted. If 1X or 2X are selected
+ *                         then a counter object will be used and the returned
+ *                         value will either exactly match the spec'd count or
+ *                         be double (2x) the spec'd count.
+ */
+void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection,
+                          EncodingType encodingType) {
+  m_table = nullptr;
+  this->channelA = channelA;
+  this->channelB = channelB;
+  m_encodingType = encodingType;
+  m_encodingScale = encodingType == k4X ? 4 : encodingType == k2X ? 2 : 1;
+
+  int index = 0;
+  m_distancePerPulse = 1.0;
+
+  LiveWindow::GetInstance()->AddSensor("Encoder", channelA, this);
+
+  if (channelB < channelA) {  // Swap ports
+    int channel = channelB;
+    channelB = channelA;
+    channelA = channel;
+    m_reverseDirection = !reverseDirection;
+  } else {
+    m_reverseDirection = reverseDirection;
+  }
+  std::stringstream ss;
+  ss << "dio/" << channelA << "/" << channelB;
+  impl = new SimEncoder(ss.str());
+  impl->Start();
+}
+
+/**
+ * Encoder constructor.
+ *
+ * Construct a Encoder given a and b channels.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aChannel         The a channel digital input channel.
+ * @param bChannel         The b channel digital input channel.
+ * @param reverseDirection If true, counts down instead of up (this is all
+ *                         relative)
+ * @param encodingType     either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ *                         decoding. If 4X is selected, then an encoder FPGA
+ *                         object is used and the returned counts will be 4x
+ *                         the encoder spec'd value since all rising and
+ *                         falling edges are counted. If 1X or 2X are selected
+ *                         then a counter object will be used and the returned
+ *                         value will either exactly match the spec'd count or
+ *                         be double (2x) the spec'd count.
+ */
+Encoder::Encoder(int aChannel, int bChannel, bool reverseDirection,
+                 EncodingType encodingType) {
+  InitEncoder(aChannel, bChannel, reverseDirection, encodingType);
+}
+
+/**
+ * Encoder constructor.
+ *
+ * Construct a Encoder given a and b channels as digital inputs. This is used in
+ * the case where the digital inputs are shared. The Encoder class will not
+ * allocate the digital inputs and assume that they already are counted.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aSource          The source that should be used for the a channel.
+ * @param bSource          the source that should be used for the b channel.
+ * @param reverseDirection If true, counts down instead of up (this is all
+ *                         relative)
+ * @param encodingType     either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ *                         decoding. If 4X is selected, then an encoder FPGA
+ *                         object is used and the returned counts will be 4x
+ *                         the encoder spec'd value since all rising and
+ *                         falling edges are counted. If 1X or 2X are selected
+ *                         then a counter object will be used and the returned
+ *                         value will either exactly match the spec'd count or
+ *                         be double (2x) the spec'd count.
+ */
+/* TODO: [Not Supported] Encoder::Encoder(DigitalSource *aSource, DigitalSource
+*bSource, bool reverseDirection, EncodingType encodingType) :
+        m_encoder(nullptr),
+        m_counter(nullptr)
+{
+        m_aSource = aSource;
+        m_bSource = bSource;
+        m_allocatedASource = false;
+        m_allocatedBSource = false;
+        if (m_aSource == nullptr || m_bSource == nullptr)
+                wpi_setWPIError(NullParameter);
+        else
+                InitEncoder(reverseDirection, encodingType);
+        }*/
+
+/**
+ * Encoder constructor.
+ *
+ * Construct a Encoder given a and b channels as digital inputs. This is used in
+ * the case where the digital inputs are shared. The Encoder class will not
+ * allocate the digital inputs and assume that they already are counted.
+ *
+ * The counter will start counting immediately.
+ *
+ * @param aSource          The source that should be used for the a channel.
+ * @param bSource          the source that should be used for the b channel.
+ * @param reverseDirection If true, counts down instead of up (this is all
+ *                         relative)
+ * @param encodingType     either k1X, k2X, or k4X to indicate 1X, 2X or 4X
+ *                         decoding. If 4X is selected, then an encoder FPGA
+ *                         object is used and the returned counts will be 4x
+ *                         the encoder spec'd value since all rising and
+ *                         falling edges are counted. If 1X or 2X are selected
+ *                         then a counter object will be used and the returned
+ *                         value will either exactly match the spec'd count or
+ *                         be double (2x) the spec'd count.
+ */
+/*// TODO: [Not Supported] Encoder::Encoder(DigitalSource &aSource,
+DigitalSource &bSource, bool reverseDirection, EncodingType encodingType) :
+        m_encoder(nullptr),
+        m_counter(nullptr)
+{
+        m_aSource = &aSource;
+        m_bSource = &bSource;
+        m_allocatedASource = false;
+        m_allocatedBSource = false;
+        InitEncoder(reverseDirection, encodingType);
+    }*/
+
+/**
+ * Reset the Encoder distance to zero.
+ *
+ * Resets the current count to zero on the encoder.
+ */
+void Encoder::Reset() { impl->Reset(); }
+
+/**
+ * Determine if the encoder is stopped.
+ *
+ * Using the MaxPeriod value, a boolean is returned that is true if the encoder
+ * is considered stopped and false if it is still moving. A stopped encoder is
+ * one where the most recent pulse width exceeds the MaxPeriod.
+ *
+ * @return True if the encoder is considered stopped.
+ */
+bool Encoder::GetStopped() const {
+  throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * The last direction the encoder value changed.
+ *
+ * @return The last direction the encoder value changed.
+ */
+bool Encoder::GetDirection() const {
+  throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * The scale needed to convert a raw counter value into a number of encoder
+ * pulses.
+ */
+double Encoder::DecodingScaleFactor() const {
+  switch (m_encodingType) {
+    case k1X:
+      return 1.0;
+    case k2X:
+      return 0.5;
+    case k4X:
+      return 0.25;
+    default:
+      return 0.0;
+  }
+}
+
+/**
+ * The encoding scale factor 1x, 2x, or 4x, per the requested encodingType.
+ *
+ * Used to divide raw edge counts down to spec'd counts.
+ */
+int Encoder::GetEncodingScale() const { return m_encodingScale; }
+
+/**
+ * Gets the raw value from the encoder.
+ *
+ * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
+ * factor.
+ *
+ * @return Current raw count from the encoder
+ */
+int Encoder::GetRaw() const {
+  throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Gets the current count.
+ *
+ * Returns the current count on the Encoder.
+ * This method compensates for the decoding type.
+ *
+ * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale
+ *         factor.
+ */
+int Encoder::Get() const {
+  throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Returns the period of the most recent pulse.
+ *
+ * Returns the period of the most recent Encoder pulse in seconds.
+ * This method compenstates for the decoding type.
+ *
+ * @deprecated Use GetRate() in favor of this method.  This returns unscaled
+ *             periods and GetRate() scales using value from
+ *             SetDistancePerPulse().
+ *
+ * @return Period in seconds of the most recent pulse.
+ */
+double Encoder::GetPeriod() const {
+  throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Sets the maximum period for stopped detection.
+ *
+ * Sets the value that represents the maximum period of the Encoder before it
+ * will assume that the attached device is stopped. This timeout allows users
+ * to determine if the wheels or other shaft has stopped rotating.
+ * This method compensates for the decoding type.
+ *
+ * @deprecated Use SetMinRate() in favor of this method.  This takes unscaled
+ *             periods and SetMinRate() scales using value from
+ *             SetDistancePerPulse().
+ *
+ * @param maxPeriod The maximum time between rising and falling edges before the
+ *                  FPGA will report the device stopped. This is expressed in
+ *                  seconds.
+ */
+void Encoder::SetMaxPeriod(double maxPeriod) {
+  throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Get the distance the robot has driven since the last reset.
+ *
+ * @return The distance driven since the last reset as scaled by the value from
+ *         SetDistancePerPulse().
+ */
+double Encoder::GetDistance() const {
+  return m_distancePerPulse * impl->GetPosition();
+}
+
+/**
+ * Get the current rate of the encoder.
+ *
+ * Units are distance per second as scaled by the value from
+ * SetDistancePerPulse().
+ *
+ * @return The current rate of the encoder.
+ */
+double Encoder::GetRate() const {
+  return m_distancePerPulse * impl->GetVelocity();
+}
+
+/**
+ * Set the minimum rate of the device before the hardware reports it stopped.
+ *
+ * @param minRate The minimum rate.  The units are in distance per second as
+ *                scaled by the value from SetDistancePerPulse().
+ */
+void Encoder::SetMinRate(double minRate) {
+  throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Set the distance per pulse for this encoder.
+ *
+ * This sets the multiplier used to determine the distance driven based on the
+ * count value from the encoder. Do not include the decoding type in this scale.
+ * The library already compensates for the decoding type. Set this value based
+ * on the encoder's rated Pulses per Revolution and factor in gearing reductions
+ * following the encoder shaft. This distance can be in any units you like,
+ * linear or angular.
+ *
+ * @param distancePerPulse The scale factor that will be used to convert pulses
+ *                         to useful units.
+ */
+void Encoder::SetDistancePerPulse(double distancePerPulse) {
+  if (m_reverseDirection) {
+    m_distancePerPulse = -distancePerPulse;
+  } else {
+    m_distancePerPulse = distancePerPulse;
+  }
+}
+
+/**
+ * Set the direction sensing for this encoder.
+ *
+ * This sets the direction sensing on the encoder so that it could count in the
+ * correct software direction regardless of the mounting.
+ *
+ * @param reverseDirection true if the encoder direction should be reversed
+ */
+void Encoder::SetReverseDirection(bool reverseDirection) {
+  throw "Simulation doesn't currently support this method.";
+}
+
+/**
+ * Set which parameter of the encoder you are using as a process control
+ * variable.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+void Encoder::SetPIDSourceType(PIDSourceType pidSource) {
+  m_pidSource = pidSource;
+}
+
+/**
+ * Implement the PIDSource interface.
+ *
+ * @return The current value of the selected source parameter.
+ */
+double Encoder::PIDGet() {
+  switch (m_pidSource) {
+    case PIDSourceType::kDisplacement:
+      return GetDistance();
+    case PIDSourceType::kRate:
+      return GetRate();
+    default:
+      return 0.0;
+  }
+}
+
+void Encoder::UpdateTable() {
+  if (m_table != nullptr) {
+    m_table->PutNumber("Speed", GetRate());
+    m_table->PutNumber("Distance", GetDistance());
+    m_table->PutNumber("Distance per Tick", m_reverseDirection
+                                                ? -m_distancePerPulse
+                                                : m_distancePerPulse);
+  }
+}
+
+void Encoder::StartLiveWindowMode() {}
+
+void Encoder::StopLiveWindowMode() {}
+
+std::string Encoder::GetSmartDashboardType() const {
+  if (m_encodingType == k4X)
+    return "Quadrature Encoder";
+  else
+    return "Encoder";
+}
+
+void Encoder::InitTable(std::shared_ptr<ITable> subTable) {
+  m_table = subTable;
+  UpdateTable();
+}
+
+std::shared_ptr<ITable> Encoder::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/GenericHID.cpp b/wpilibc/sim/src/GenericHID.cpp
new file mode 100644
index 0000000..c76e03d
--- /dev/null
+++ b/wpilibc/sim/src/GenericHID.cpp
@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. 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 "GenericHID.h"
+
+#include "DriverStation.h"
+
+using namespace frc;
+
+GenericHID::GenericHID(int port) : m_ds(DriverStation::GetInstance()) {
+  m_port = port;
+}
+
+/**
+ * Get the value of the axis.
+ *
+ * @param axis The axis to read, starting at 0.
+ * @return The value of the axis.
+ */
+double GenericHID::GetRawAxis(int axis) const {
+  return m_ds.GetStickAxis(m_port, axis);
+}
+
+/**
+ * Get the button value (starting at button 1)
+ *
+ * The buttons are returned in a single 16 bit value with one bit representing
+ * the state of each button. The appropriate button is returned as a boolean
+ * value.
+ *
+ * @param button The button number to be read (starting at 1)
+ * @return The state of the button.
+ **/
+bool GenericHID::GetRawButton(int button) const {
+  return m_ds.GetStickButton(m_port, button);
+}
+
+/**
+ * Get the angle in degrees of a POV on the HID.
+ *
+ * The POV angles start at 0 in the up direction, and increase clockwise
+ * (e.g. right is 90, upper-left is 315).
+ *
+ * @param pov The index of the POV to read (starting at 0)
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+int GenericHID::GetPOV(int pov) const { return 0; }
+
+/**
+ * Get the number of POVs for the HID.
+ *
+ * @return the number of POVs for the current HID
+ */
+int GenericHID::GetPOVCount() const { return 0; }
+
+/**
+ * Get the port number of the HID.
+ *
+ * @return The port number of the HID.
+ */
+int GenericHID::GetPort() const { return m_port; }
+
+/**
+ * Get the type of the HID.
+ *
+ * @return the type of the HID.
+ */
+GenericHID::HIDType GenericHID::GetType() const { return HIDType::kUnknown; }
+
+/**
+ * Get the name of the HID.
+ *
+ * @return the name of the HID.
+ */
+std::string GenericHID::GetName() const { return ""; }
+
+/**
+ * Set a single HID output value for the HID.
+ *
+ * @param outputNumber The index of the output to set (1-32)
+ * @param value        The value to set the output to
+ */
+
+void GenericHID::SetOutput(int outputNumber, bool value) {
+  m_outputs =
+      (m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
+}
+
+/**
+ * Set all output values for the HID.
+ *
+ * @param value The 32 bit output value (1 bit for each output)
+ */
+void GenericHID::SetOutputs(int value) { m_outputs = value; }
+
+/**
+ * Set the rumble output for the HID.
+ *
+ * The DS currently supports 2 rumble values, left rumble and right rumble.
+ *
+ * @param type  Which rumble value to set
+ * @param value The normalized value (0 to 1) to set the rumble to
+ */
+void GenericHID::SetRumble(RumbleType type, double value) {
+  if (value < 0)
+    value = 0;
+  else if (value > 1)
+    value = 1;
+  if (type == kLeftRumble) {
+    m_leftRumble = value * 65535;
+  } else {
+    m_rightRumble = value * 65535;
+  }
+}
diff --git a/wpilibc/sim/src/IterativeRobot.cpp b/wpilibc/sim/src/IterativeRobot.cpp
new file mode 100644
index 0000000..9a37c05
--- /dev/null
+++ b/wpilibc/sim/src/IterativeRobot.cpp
@@ -0,0 +1,223 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "IterativeRobot.h"
+
+#include "DriverStation.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+// not sure what this is used for yet.
+#ifdef _UNIX
+#include <unistd.h>
+#endif
+
+using namespace frc;
+
+/**
+ * Provide an alternate "main loop" via StartCompetition().
+ *
+ * This specific StartCompetition() implements "main loop" behavior like that of
+ * the FRC control system in 2008 and earlier, with a primary (slow) loop that
+ * is called periodically, and a "fast loop" (a.k.a. "spin loop") that is
+ * called as fast as possible with no delay between calls.
+ */
+void IterativeRobot::StartCompetition() {
+  LiveWindow* lw = LiveWindow::GetInstance();
+  // first and one-time initialization
+  NetworkTable::GetTable("LiveWindow")
+      ->GetSubTable("~STATUS~")
+      ->PutBoolean("LW Enabled", false);
+  RobotInit();
+
+  // loop forever, calling the appropriate mode-dependent function
+  lw->SetEnabled(false);
+  while (true) {
+    // Call the appropriate function depending upon the current robot mode
+    if (IsDisabled()) {
+      // call DisabledInit() if we are now just entering disabled mode from
+      // either a different mode or from power-on
+      if (!m_disabledInitialized) {
+        lw->SetEnabled(false);
+        DisabledInit();
+        m_disabledInitialized = true;
+        // reset the initialization flags for the other modes
+        m_autonomousInitialized = false;
+        m_teleopInitialized = false;
+        m_testInitialized = false;
+      }
+      // TODO: HALNetworkCommunicationObserveUserProgramDisabled();
+      DisabledPeriodic();
+    } else if (IsAutonomous()) {
+      // call AutonomousInit() if we are now just entering autonomous mode from
+      // either a different mode or from power-on
+      if (!m_autonomousInitialized) {
+        lw->SetEnabled(false);
+        AutonomousInit();
+        m_autonomousInitialized = true;
+        // reset the initialization flags for the other modes
+        m_disabledInitialized = false;
+        m_teleopInitialized = false;
+        m_testInitialized = false;
+      }
+      // TODO: HALNetworkCommunicationObserveUserProgramAutonomous();
+      AutonomousPeriodic();
+    } else if (IsTest()) {
+      // call TestInit() if we are now just entering test mode from
+      // either a different mode or from power-on
+      if (!m_testInitialized) {
+        lw->SetEnabled(true);
+        TestInit();
+        m_testInitialized = true;
+        // reset the initialization flags for the other modes
+        m_disabledInitialized = false;
+        m_autonomousInitialized = false;
+        m_teleopInitialized = false;
+      }
+      // TODO: HALNetworkCommunicationObserveUserProgramTest();
+      TestPeriodic();
+    } else {
+      // call TeleopInit() if we are now just entering teleop mode from
+      // either a different mode or from power-on
+      if (!m_teleopInitialized) {
+        lw->SetEnabled(false);
+        TeleopInit();
+        m_teleopInitialized = true;
+        // reset the initialization flags for the other modes
+        m_disabledInitialized = false;
+        m_autonomousInitialized = false;
+        m_testInitialized = false;
+        Scheduler::GetInstance()->SetEnabled(true);
+      }
+      // TODO: HALNetworkCommunicationObserveUserProgramTeleop();
+      TeleopPeriodic();
+    }
+    // wait for driver station data so the loop doesn't hog the CPU
+    m_ds.WaitForData();
+  }
+}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Users should override this method for default Robot-wide initialization which
+ * will be called when the robot is first powered on.  It will be called
+ * exactly 1 time.
+ */
+void IterativeRobot::RobotInit() {
+  std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for disabled mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters disabled mode.
+ */
+void IterativeRobot::DisabledInit() {
+  std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for autonomous mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters autonomous mode.
+ */
+void IterativeRobot::AutonomousInit() {
+  std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for teleop mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters teleop mode.
+ */
+void IterativeRobot::TeleopInit() {
+  std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Initialization code for test mode should go here.
+ *
+ * Users should override this method for initialization code which will be
+ * called each time the robot enters test mode.
+ */
+void IterativeRobot::TestInit() {
+  std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * Periodic code for all modes should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in any mode.
+ */
+void IterativeRobot::RobotPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+/**
+ * Periodic code for disabled mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in disabled mode.
+ */
+void IterativeRobot::DisabledPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+/**
+ * Periodic code for autonomous mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in autonomous mode.
+ */
+void IterativeRobot::AutonomousPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+/**
+ * Periodic code for teleop mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in teleop mode.
+ */
+void IterativeRobot::TeleopPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
+
+/**
+ * Periodic code for test mode should go here.
+ *
+ * Users should override this method for code which will be called periodically
+ * at a regular rate while the robot is in test mode.
+ */
+void IterativeRobot::TestPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    std::printf("Default %s() method... Overload me!\n", __FUNCTION__);
+    firstRun = false;
+  }
+}
diff --git a/wpilibc/sim/src/Jaguar.cpp b/wpilibc/sim/src/Jaguar.cpp
new file mode 100644
index 0000000..84c3470
--- /dev/null
+++ b/wpilibc/sim/src/Jaguar.cpp
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "LiveWindow/LiveWindow.h"
+
+using namespace frc;
+
+/**
+ * @param channel The PWM channel that the Jaguar is attached to.
+ */
+Jaguar::Jaguar(int channel) : SafePWM(channel) {
+  /*
+   * Input profile defined by Luminary Micro.
+   *
+   * Full reverse ranges from 0.671325ms to 0.6972211ms
+   * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
+   * Neutral ranges from 1.4482078ms to 1.5517922ms
+   * Proportional forward ranges from 1.5517922ms to 2.3027789ms
+   * Full forward ranges from 2.3027789ms to 2.328675ms
+   */
+  SetBounds(2.31, 1.55, 1.507, 1.454, .697);
+  SetPeriodMultiplier(kPeriodMultiplier_1X);
+  SetRaw(m_centerPwm);
+
+  LiveWindow::GetInstance()->AddActuator("Jaguar", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+void Jaguar::Set(double speed) { SetSpeed(speed); }
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+double Jaguar::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Jaguar::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Jaguar::PIDWrite(double output) { Set(output); }
diff --git a/wpilibc/sim/src/Joystick.cpp b/wpilibc/sim/src/Joystick.cpp
new file mode 100644
index 0000000..fc697b1
--- /dev/null
+++ b/wpilibc/sim/src/Joystick.cpp
@@ -0,0 +1,274 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "Joystick.h"
+
+#include <cmath>
+
+#include "DriverStation.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const int Joystick::kDefaultXAxis;
+const int Joystick::kDefaultYAxis;
+const int Joystick::kDefaultZAxis;
+const int Joystick::kDefaultTwistAxis;
+const int Joystick::kDefaultThrottleAxis;
+const int Joystick::kDefaultTriggerButton;
+const int Joystick::kDefaultTopButton;
+static Joystick* joysticks[DriverStation::kJoystickPorts];
+static bool joySticksInitialized = false;
+
+/**
+ * Construct an instance of a joystick.
+ *
+ * The joystick index is the USB port on the Driver Station.
+ *
+ * @param port The port on the Driver Station that the joystick is plugged into
+ *             (0-5).
+ */
+Joystick::Joystick(int port) : Joystick(port, kNumAxisTypes, kNumButtonTypes) {
+  m_axes[kXAxis] = kDefaultXAxis;
+  m_axes[kYAxis] = kDefaultYAxis;
+  m_axes[kZAxis] = kDefaultZAxis;
+  m_axes[kTwistAxis] = kDefaultTwistAxis;
+  m_axes[kThrottleAxis] = kDefaultThrottleAxis;
+
+  m_buttons[kTriggerButton] = kDefaultTriggerButton;
+  m_buttons[kTopButton] = kDefaultTopButton;
+}
+
+/**
+ * Version of the constructor to be called by sub-classes.
+ *
+ * This constructor allows the subclass to configure the number of constants
+ * for axes and buttons.
+ *
+ * @param port           The port on the Driver Station that the joystick is
+ *                       plugged into.
+ * @param numAxisTypes   The number of axis types in the enum.
+ * @param numButtonTypes The number of button types in the enum.
+ */
+Joystick::Joystick(int port, int numAxisTypes, int numButtonTypes)
+    : JoystickBase(port),
+      m_ds(DriverStation::GetInstance()),
+      m_axes(numAxisTypes),
+      m_buttons(numButtonTypes) {
+  if (!joySticksInitialized) {
+    for (auto& joystick : joysticks) joystick = nullptr;
+    joySticksInitialized = true;
+  }
+  if (GetPort() >= DriverStation::kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+  } else {
+    joysticks[GetPort()] = this;
+  }
+}
+
+Joystick* Joystick::GetStickForPort(int port) {
+  Joystick* stick = joysticks[port];
+  if (stick == nullptr) {
+    stick = new Joystick(port);
+    joysticks[port] = stick;
+  }
+  return stick;
+}
+
+/**
+ * Get the X value of the joystick.
+ *
+ * This depends on the mapping of the joystick connected to the current port.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only
+ *             here to complete the GenericHID interface.
+ */
+double Joystick::GetX(JoystickHand hand) const {
+  return GetRawAxis(m_axes[kXAxis]);
+}
+
+/**
+ * Get the Y value of the joystick.
+ *
+ * This depends on the mapping of the joystick connected to the current port.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only
+ *             here to complete the GenericHID interface.
+ */
+double Joystick::GetY(JoystickHand hand) const {
+  return GetRawAxis(m_axes[kYAxis]);
+}
+
+/**
+ * Get the Z value of the current joystick.
+ *
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+double Joystick::GetZ(JoystickHand hand) const {
+  return GetRawAxis(m_axes[kZAxis]);
+}
+
+/**
+ * Get the twist value of the current joystick.
+ *
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+double Joystick::GetTwist() const { return GetRawAxis(m_axes[kTwistAxis]); }
+
+/**
+ * Get the throttle value of the current joystick.
+ *
+ * This depends on the mapping of the joystick connected to the current port.
+ */
+double Joystick::GetThrottle() const {
+  return GetRawAxis(m_axes[kThrottleAxis]);
+}
+
+/**
+ * For the current joystick, return the axis determined by the argument.
+ *
+ * This is for cases where the joystick axis is returned programatically,
+ * otherwise one of the previous functions would be preferable (for example
+ * GetX()).
+ *
+ * @param axis The axis to read.
+ * @return The value of the axis.
+ */
+double Joystick::GetAxis(AxisType axis) const {
+  switch (axis) {
+    case kXAxis:
+      return this->GetX();
+    case kYAxis:
+      return this->GetY();
+    case kZAxis:
+      return this->GetZ();
+    case kTwistAxis:
+      return this->GetTwist();
+    case kThrottleAxis:
+      return this->GetThrottle();
+    default:
+      wpi_setWPIError(BadJoystickAxis);
+      return 0.0;
+  }
+}
+
+/**
+ * Read the state of the trigger on the joystick.
+ *
+ * Look up which button has been assigned to the trigger and read its state.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only
+ *             here to complete the GenericHID interface.
+ * @return The state of the trigger.
+ */
+bool Joystick::GetTrigger(JoystickHand hand) const {
+  return GetRawButton(m_buttons[kTriggerButton]);
+}
+
+/**
+ * Read the state of the top button on the joystick.
+ *
+ * Look up which button has been assigned to the top and read its state.
+ *
+ * @param hand This parameter is ignored for the Joystick class and is only
+ *             here to complete the GenericHID interface.
+ * @return The state of the top button.
+ */
+bool Joystick::GetTop(JoystickHand hand) const {
+  return GetRawButton(m_buttons[kTopButton]);
+}
+
+/**
+ * Get buttons based on an enumerated type.
+ *
+ * The button type will be looked up in the list of buttons and then read.
+ *
+ * @param button The type of button to read.
+ * @return The state of the button.
+ */
+bool Joystick::GetButton(ButtonType button) const {
+  switch (button) {
+    case kTriggerButton:
+      return GetTrigger();
+    case kTopButton:
+      return GetTop();
+    default:
+      return false;
+  }
+}
+
+/**
+ * Get the number of axis for a joystick
+ *
+ * @return the number of axis for the current joystick
+ */
+int Joystick::GetAxisCount() const { return 0; }
+
+/**
+ * Get the axis type of a joystick axis.
+ *
+ * @return the axis type of a joystick axis.
+ */
+int Joystick::GetAxisType(int axis) const { return 0; }
+
+/**
+ * Get the number of buttons for a joystick.
+ *
+ * @return the number of buttons on the current joystick
+ */
+int Joystick::GetButtonCount() const { return 0; }
+
+/**
+ * Get the channel currently associated with the specified axis.
+ *
+ * @param axis The axis to look up the channel for.
+ * @return The channel fr the axis.
+ */
+int Joystick::GetAxisChannel(AxisType axis) const { return m_axes[axis]; }
+
+/**
+ * Set the channel associated with a specified axis.
+ *
+ * @param axis    The axis to set the channel for.
+ * @param channel The channel to set the axis to.
+ */
+void Joystick::SetAxisChannel(AxisType axis, int channel) {
+  m_axes[axis] = channel;
+}
+
+/**
+ * Get the magnitude of the direction vector formed by the joystick's
+ * current position relative to its origin.
+ *
+ * @return The magnitude of the direction vector
+ */
+double Joystick::GetMagnitude() const {
+  return std::sqrt(std::pow(GetX(), 2) + std::pow(GetY(), 2));
+}
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in radians.
+ *
+ * @return The direction of the vector in radians
+ */
+double Joystick::GetDirectionRadians() const {
+  return std::atan2(GetX(), -GetY());
+}
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in degrees.
+ *
+ * uses std::acos(-1) to represent Pi due to absence of readily accessible Pi
+ * constant in C++
+ *
+ * @return The direction of the vector in degrees
+ */
+double Joystick::GetDirectionDegrees() const {
+  return (180 / std::acos(-1)) * GetDirectionRadians();
+}
diff --git a/wpilibc/sim/src/MotorSafetyHelper.cpp b/wpilibc/sim/src/MotorSafetyHelper.cpp
new file mode 100644
index 0000000..df4ecde
--- /dev/null
+++ b/wpilibc/sim/src/MotorSafetyHelper.cpp
@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "MotorSafetyHelper.h"
+
+#include <sstream>
+
+#include "DriverStation.h"
+#include "MotorSafety.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+std::set<MotorSafetyHelper*> MotorSafetyHelper::m_helperList;
+priority_recursive_mutex MotorSafetyHelper::m_listMutex;
+
+/**
+ * The constructor for a MotorSafetyHelper object.
+ *
+ * The helper object is constructed for every object that wants to implement the
+ * Motor Safety protocol. The helper object has the code to actually do the
+ * timing and call the motors Stop() method when the timeout expires. The motor
+ * object is expected to call the Feed() method whenever the motors value is
+ * updated.
+ *
+ * @param safeObject a pointer to the motor object implementing MotorSafety.
+ *                   This is used to call the Stop() method on the motor.
+ */
+MotorSafetyHelper::MotorSafetyHelper(MotorSafety* safeObject)
+    : m_safeObject(safeObject) {
+  m_enabled = false;
+  m_expiration = DEFAULT_SAFETY_EXPIRATION;
+  m_stopTime = Timer::GetFPGATimestamp();
+
+  std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+  m_helperList.insert(this);
+}
+
+MotorSafetyHelper::~MotorSafetyHelper() {
+  std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+  m_helperList.erase(this);
+}
+
+/**
+ * Feed the motor safety object.
+ *
+ * Resets the timer on this object that is used to do the timeouts.
+ */
+void MotorSafetyHelper::Feed() {
+  std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+  m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
+}
+
+/**
+ * Set the expiration time for the corresponding motor safety object.
+ *
+ * @param expirationTime The timeout value in seconds.
+ */
+void MotorSafetyHelper::SetExpiration(double expirationTime) {
+  std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+  m_expiration = expirationTime;
+}
+
+/**
+ * Retrieve the timeout value for the corresponding motor safety object.
+ *
+ * @return the timeout value in seconds.
+ */
+double MotorSafetyHelper::GetExpiration() const {
+  std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+  return m_expiration;
+}
+
+/**
+ * Determine if the motor is still operating or has timed out.
+ *
+ * @return a true value if the motor is still operating normally and hasn't
+ *         timed out.
+ */
+bool MotorSafetyHelper::IsAlive() const {
+  std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+  return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
+}
+
+/**
+ * Check if this motor has exceeded its timeout.
+ *
+ * This method is called periodically to determine if this motor has exceeded
+ * its timeout value. If it has, the stop method is called, and the motor is
+ * shut down until its value is updated again.
+ */
+void MotorSafetyHelper::Check() {
+  DriverStation& ds = DriverStation::GetInstance();
+  if (!m_enabled || ds.IsDisabled() || ds.IsTest()) return;
+
+  std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+  if (m_stopTime < Timer::GetFPGATimestamp()) {
+    std::ostringstream desc;
+    m_safeObject->GetDescription(desc);
+    desc << "... Output not updated often enough.";
+    wpi_setWPIErrorWithContext(Timeout, desc.str().c_str());
+    m_safeObject->StopMotor();
+  }
+}
+
+/**
+ * Enable/disable motor safety for this device.
+ *
+ * Turn on and off the motor safety option for this PWM object.
+ *
+ * @param enabled True if motor safety is enforced for this object
+ */
+void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
+  std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+  m_enabled = enabled;
+}
+
+/**
+ * Return the state of the motor safety enabled flag.
+ *
+ * Return if the motor safety is currently enabled for this devicce.
+ *
+ * @return True if motor safety is enforced for this device
+ */
+bool MotorSafetyHelper::IsSafetyEnabled() const {
+  std::lock_guard<priority_recursive_mutex> sync(m_syncMutex);
+  return m_enabled;
+}
+
+/**
+ * Check the motors to see if any have timed out.
+ *
+ * This static method is called periodically to poll all the motors and stop
+ * any that have timed out.
+ */
+void MotorSafetyHelper::CheckMotors() {
+  std::lock_guard<priority_recursive_mutex> sync(m_listMutex);
+  for (auto elem : m_helperList) {
+    elem->Check();
+  }
+}
diff --git a/wpilibc/sim/src/Notifier.cpp b/wpilibc/sim/src/Notifier.cpp
new file mode 100644
index 0000000..7950dae
--- /dev/null
+++ b/wpilibc/sim/src/Notifier.cpp
@@ -0,0 +1,267 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "Utility.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+std::list<Notifier*> Notifier::timerQueue;
+priority_recursive_mutex Notifier::queueMutex;
+std::atomic<int> Notifier::refcount{0};
+std::thread Notifier::m_task;
+std::atomic<bool> Notifier::m_stopped(false);
+
+/**
+ * Create a Notifier for timer event notification.
+ *
+ * @param handler The handler is called at the notification time which is set
+ *                using StartSingle or StartPeriodic.
+ */
+Notifier::Notifier(TimerEventHandler handler) {
+  if (handler == nullptr)
+    wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+  m_handler = handler;
+  m_periodic = false;
+  m_expirationTime = 0;
+  m_period = 0;
+  m_queued = false;
+  {
+    std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+    // do the first time intialization of static variables
+    if (refcount.fetch_add(1) == 0) {
+      m_task = std::thread(Run);
+    }
+  }
+}
+
+/**
+ * Free the resources for a timer event.
+ *
+ * All resources will be freed and the timer event will be removed from the
+ * queue if necessary.
+ */
+Notifier::~Notifier() {
+  {
+    std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+    DeleteFromQueue();
+
+    // Delete the static variables when the last one is going away
+    if (refcount.fetch_sub(1) == 1) {
+      m_stopped = true;
+      m_task.join();
+    }
+  }
+
+  // Acquire the semaphore; this makes certain that the handler is
+  // not being executed by the interrupt manager.
+  std::lock_guard<priority_mutex> lock(m_handlerMutex);
+}
+
+/**
+ * Update the alarm hardware to reflect the current first element in the queue.
+ *
+ * Compute the time the next alarm should occur based on the current time and
+ * the period for the first element in the timer queue.
+ *
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere that is taking care of synchronizing access to the queue.
+ */
+void Notifier::UpdateAlarm() {}
+
+/**
+ * ProcessQueue is called whenever there is a timer interrupt.
+ *
+ * We need to wake up and process the current top item in the timer queue as
+ * long as its scheduled time is after the current time. Then the item is
+ * removed or rescheduled (repetitive events) in the queue.
+ */
+void Notifier::ProcessQueue(int mask, void* params) {
+  Notifier* current;
+
+  // keep processing events until no more
+  while (true) {
+    {
+      std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+      double currentTime = GetClock();
+
+      if (timerQueue.empty()) {
+        break;
+      }
+      current = timerQueue.front();
+      if (current->m_expirationTime > currentTime) {
+        break;  // no more timer events to process
+      }
+      // remove next entry before processing it
+      timerQueue.pop_front();
+
+      current->m_queued = false;
+      if (current->m_periodic) {
+        // if periodic, requeue the event
+        // compute when to put into queue
+        current->InsertInQueue(true);
+      } else {
+        // not periodic; removed from queue
+        current->m_queued = false;
+      }
+      // Take handler mutex while holding queue semaphore to make sure
+      //  the handler will execute to completion in case we are being deleted.
+      current->m_handlerMutex.lock();
+    }
+
+    current->m_handler();  // call the event handler
+    current->m_handlerMutex.unlock();
+  }
+  // reschedule the first item in the queue
+  std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+  UpdateAlarm();
+}
+
+/**
+ * Insert this Notifier into the timer queue in right place.
+ *
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere that is taking care of synchronizing access to the queue.
+ *
+ * @param reschedule If false, the scheduled alarm is based on the curent time
+ *                   and UpdateAlarm method is called which will enable the
+ *                   alarm if necessary. If true, update the time by adding the
+ *                   period (no drift) when rescheduled periodic from
+ *                   ProcessQueue.
+ *
+ * This ensures that the public methods only update the queue after finishing
+ * inserting.
+ */
+void Notifier::InsertInQueue(bool reschedule) {
+  if (reschedule) {
+    m_expirationTime += m_period;
+  } else {
+    m_expirationTime = GetClock() + m_period;
+  }
+
+  // Attempt to insert new entry into queue
+  for (auto i = timerQueue.begin(); i != timerQueue.end(); i++) {
+    if ((*i)->m_expirationTime > m_expirationTime) {
+      timerQueue.insert(i, this);
+      m_queued = true;
+    }
+  }
+
+  /* If the new entry wasn't queued, either the queue was empty or the first
+   * element was greater than the new entry.
+   */
+  if (!m_queued) {
+    timerQueue.push_front(this);
+
+    if (!reschedule) {
+      /* Since the first element changed, update alarm, unless we already
+       * plan to
+       */
+      UpdateAlarm();
+    }
+
+    m_queued = true;
+  }
+}
+
+/**
+ * Delete this Notifier from the timer queue.
+ *
+ * WARNING: this method does not do synchronization! It must be called from
+ * somewhere that is taking care of synchronizing access to the queue.
+ *
+ * Remove this Notifier from the timer queue and adjust the next interrupt time
+ * to reflect the current top of the queue.
+ */
+void Notifier::DeleteFromQueue() {
+  if (m_queued) {
+    m_queued = false;
+    wpi_assert(!timerQueue.empty());
+    if (timerQueue.front() == this) {
+      // remove the first item in the list - update the alarm
+      timerQueue.pop_front();
+      UpdateAlarm();
+    } else {
+      timerQueue.remove(this);
+    }
+  }
+}
+
+/**
+ * Register for single event notification.
+ *
+ * A timer event is queued for a single event after the specified delay.
+ *
+ * @param delay Seconds to wait before the handler is called.
+ */
+void Notifier::StartSingle(double delay) {
+  std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+  m_periodic = false;
+  m_period = delay;
+  DeleteFromQueue();
+  InsertInQueue(false);
+}
+
+/**
+ * Register for periodic event notification.
+ *
+ * A timer event is queued for periodic event notification. Each time the
+ * interrupt occurs, the event will be immediately requeued for the same time
+ * interval.
+ *
+ * @param period Period in seconds to call the handler starting one period after
+ *               the call to this method.
+ */
+void Notifier::StartPeriodic(double period) {
+  std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+  m_periodic = true;
+  m_period = period;
+  DeleteFromQueue();
+  InsertInQueue(false);
+}
+
+/**
+ * Stop timer events from occuring.
+ *
+ * Stop any repeating timer events from occuring. This will also remove any
+ * single notification events from the queue. If a timer-based call to the
+ * registered handler is in progress, this function will block until the
+ * handler call is complete.
+ */
+void Notifier::Stop() {
+  {
+    std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+    DeleteFromQueue();
+  }
+  // Wait for a currently executing handler to complete before returning from
+  // Stop()
+  std::lock_guard<priority_mutex> sync(m_handlerMutex);
+}
+
+void Notifier::Run() {
+  while (!m_stopped) {
+    Notifier::ProcessQueue(0, nullptr);
+    bool isEmpty;
+    {
+      std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+      isEmpty = timerQueue.empty();
+    }
+    if (!isEmpty) {
+      double expirationTime;
+      {
+        std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+        expirationTime = timerQueue.front()->m_expirationTime;
+      }
+      Wait(expirationTime - GetClock());
+    } else {
+      Wait(0.05);
+    }
+  }
+}
diff --git a/wpilibc/sim/src/PIDController.cpp b/wpilibc/sim/src/PIDController.cpp
new file mode 100644
index 0000000..0231eb8
--- /dev/null
+++ b/wpilibc/sim/src/PIDController.cpp
@@ -0,0 +1,643 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "PIDController.h"
+
+#include <cmath>
+
+#include "Notifier.h"
+#include "PIDOutput.h"
+#include "PIDSource.h"
+
+using namespace frc;
+
+static const std::string kP = "p";
+static const std::string kI = "i";
+static const std::string kD = "d";
+static const std::string kF = "f";
+static const std::string kSetpoint = "setpoint";
+static const std::string kEnabled = "enabled";
+
+/**
+ * Allocate a PID object with the given constants for P, I, D.
+ *
+ * @param Kp     the proportional coefficient
+ * @param Ki     the integral coefficient
+ * @param Kd     the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output value
+ * @param period the loop time for doing calculations. This particularly effects
+ *               calculations of the integral and differental terms. The
+ *               default is 50ms.
+ */
+PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source,
+                             PIDOutput* output, double period)
+    : PIDController(Kp, Ki, Kd, 0.0, source, output, period) {}
+
+/**
+ * Allocate a PID object with the given constants for P, I, D.
+ *
+ * @param Kp     the proportional coefficient
+ * @param Ki     the integral coefficient
+ * @param Kd     the derivative coefficient
+ * @param source The PIDSource object that is used to get values
+ * @param output The PIDOutput object that is set to the output value
+ * @param period the loop time for doing calculations. This particularly effects
+ *               calculations of the integral and differental terms. The
+ *               default is 50ms.
+ */
+PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
+                             PIDSource* source, PIDOutput* output,
+                             double period) {
+  m_table = nullptr;
+
+  m_P = Kp;
+  m_I = Ki;
+  m_D = Kd;
+  m_F = Kf;
+
+  m_maximumOutput = 1.0;
+  m_minimumOutput = -1.0;
+
+  m_maximumInput = 0;
+  m_minimumInput = 0;
+
+  m_continuous = false;
+  m_enabled = false;
+  m_setpoint = 0;
+
+  m_prevError = 0;
+  m_totalError = 0;
+  m_tolerance = .05;
+
+  m_result = 0;
+
+  m_pidInput = source;
+  m_pidOutput = output;
+  m_period = period;
+
+  m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
+  m_controlLoop->StartPeriodic(m_period);
+
+  static int instances = 0;
+  instances++;
+
+  m_toleranceType = kNoTolerance;
+}
+
+PIDController::~PIDController() {
+  if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Read the input, calculate the output accordingly, and write to the output.
+ *
+ * This should only be called by the Notifier.
+ */
+void PIDController::Calculate() {
+  bool enabled;
+  PIDSource* pidInput;
+
+  {
+    std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+    if (m_pidInput == 0) return;
+    if (m_pidOutput == 0) return;
+    enabled = m_enabled;
+    pidInput = m_pidInput;
+  }
+
+  if (enabled) {
+    double input = pidInput->PIDGet();
+    double result;
+    PIDOutput* pidOutput;
+
+    {
+      std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+      m_error = m_setpoint - input;
+      if (m_continuous) {
+        if (std::fabs(m_error) > (m_maximumInput - m_minimumInput) / 2) {
+          if (m_error > 0) {
+            m_error = m_error - (m_maximumInput - m_minimumInput);
+          } else {
+            m_error = m_error + (m_maximumInput - m_minimumInput);
+          }
+        }
+      }
+
+      if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+        if (m_P != 0) {
+          double potentialPGain = (m_totalError + m_error) * m_P;
+          if (potentialPGain < m_maximumOutput) {
+            if (potentialPGain > m_minimumOutput) {
+              m_totalError += m_error;
+            } else {
+              m_totalError = m_minimumOutput / m_P;
+            }
+          } else {
+            m_totalError = m_maximumOutput / m_P;
+          }
+        }
+
+        m_result = m_D * m_error + m_P * m_totalError + CalculateFeedForward();
+      } else {
+        if (m_I != 0) {
+          double potentialIGain = (m_totalError + m_error) * m_I;
+          if (potentialIGain < m_maximumOutput) {
+            if (potentialIGain > m_minimumOutput) {
+              m_totalError += m_error;
+            } else {
+              m_totalError = m_minimumOutput / m_I;
+            }
+          } else {
+            m_totalError = m_maximumOutput / m_I;
+          }
+        }
+
+        m_result = m_P * m_error + m_I * m_totalError +
+                   m_D * (m_error - m_prevError) + CalculateFeedForward();
+      }
+      m_prevError = m_error;
+
+      if (m_result > m_maximumOutput)
+        m_result = m_maximumOutput;
+      else if (m_result < m_minimumOutput)
+        m_result = m_minimumOutput;
+
+      pidOutput = m_pidOutput;
+      result = m_result;
+    }
+
+    pidOutput->PIDWrite(result);
+  }
+}
+
+/**
+ * Calculate the feed forward term.
+ *
+ * Both of the provided feed forward calculations are velocity feed forwards.
+ * If a different feed forward calculation is desired, the user can override
+ * this function and provide his or her own. This function does no
+ * synchronization because the PIDController class only calls it in synchronized
+ * code, so be careful if calling it oneself.
+ *
+ * If a velocity PID controller is being used, the F term should be set to 1
+ * over the maximum setpoint for the output. If a position PID controller is
+ * being used, the F term should be set to 1 over the maximum speed for the
+ * output measured in setpoint units per this controller's update period (see
+ * the default period in this class's constructor).
+ */
+double PIDController::CalculateFeedForward() {
+  if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+    return m_F * GetSetpoint();
+  } else {
+    double temp = m_F * GetDeltaSetpoint();
+    m_prevSetpoint = m_setpoint;
+    m_setpointTimer.Reset();
+    return temp;
+  }
+}
+
+/**
+ * Set the PID Controller gain parameters.
+ *
+ * Set the proportional, integral, and differential coefficients.
+ *
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ */
+void PIDController::SetPID(double p, double i, double d) {
+  {
+    std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+    m_P = p;
+    m_I = i;
+    m_D = d;
+  }
+
+  if (m_table != nullptr) {
+    m_table->PutNumber("p", m_P);
+    m_table->PutNumber("i", m_I);
+    m_table->PutNumber("d", m_D);
+  }
+}
+
+/**
+ * Set the PID Controller gain parameters.
+ *
+ * Set the proportional, integral, and differential coefficients.
+ *
+ * @param p Proportional coefficient
+ * @param i Integral coefficient
+ * @param d Differential coefficient
+ * @param f Feed forward coefficient
+ */
+void PIDController::SetPID(double p, double i, double d, double f) {
+  {
+    std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+    m_P = p;
+    m_I = i;
+    m_D = d;
+    m_F = f;
+  }
+
+  if (m_table != nullptr) {
+    m_table->PutNumber("p", m_P);
+    m_table->PutNumber("i", m_I);
+    m_table->PutNumber("d", m_D);
+    m_table->PutNumber("f", m_F);
+  }
+}
+
+/**
+ * Get the Proportional coefficient.
+ *
+ * @return proportional coefficient
+ */
+double PIDController::GetP() const {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  return m_P;
+}
+
+/**
+ * Get the Integral coefficient.
+ *
+ * @return integral coefficient
+ */
+double PIDController::GetI() const {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  return m_I;
+}
+
+/**
+ * Get the Differential coefficient.
+ *
+ * @return differential coefficient
+ */
+double PIDController::GetD() const {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  return m_D;
+}
+
+/**
+ * Get the Feed forward coefficient.
+ *
+ * @return Feed forward coefficient
+ */
+double PIDController::GetF() const {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  return m_F;
+}
+
+/**
+ * Return the current PID result.
+ *
+ * This is always centered on zero and constrained the the max and min outs.
+ *
+ * @return the latest calculated output
+ */
+double PIDController::Get() const {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  return m_result;
+}
+
+/**
+ * Set the PID controller to consider the input to be continuous.
+ *
+ * Rather then using the max and min in as constraints, it considers them to
+ * be the same point and automatically calculates the shortest route to
+ * the setpoint.
+ *
+ * @param continuous Set to true turns on continuous, false turns off continuous
+ */
+void PIDController::SetContinuous(bool continuous) {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  m_continuous = continuous;
+}
+
+/**
+ * Sets the maximum and minimum values expected from the input.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the output
+ */
+void PIDController::SetInputRange(double minimumInput, double maximumInput) {
+  {
+    std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  SetSetpoint(m_setpoint);
+}
+
+/**
+ * Sets the minimum and maximum values to write.
+ *
+ * @param minimumOutput the minimum value to write to the output
+ * @param maximumOutput the maximum value to write to the output
+ */
+void PIDController::SetOutputRange(double minimumOutput, double maximumOutput) {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  m_minimumOutput = minimumOutput;
+  m_maximumOutput = maximumOutput;
+}
+
+/**
+ * Set the setpoint for the PIDController.
+ *
+ * @param setpoint the desired setpoint
+ */
+void PIDController::SetSetpoint(double setpoint) {
+  {
+    std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+
+    if (m_maximumInput > m_minimumInput) {
+      if (setpoint > m_maximumInput)
+        m_setpoint = m_maximumInput;
+      else if (setpoint < m_minimumInput)
+        m_setpoint = m_minimumInput;
+      else
+        m_setpoint = setpoint;
+    } else {
+      m_setpoint = setpoint;
+    }
+  }
+
+  if (m_table != nullptr) {
+    m_table->PutNumber("setpoint", m_setpoint);
+  }
+}
+
+/**
+ * Returns the current setpoint of the PIDController.
+ *
+ * @return the current setpoint
+ */
+double PIDController::GetSetpoint() const {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  return m_setpoint;
+}
+
+/**
+ * Returns the change in setpoint over time of the PIDController.
+ *
+ * @return the change in setpoint over time
+ */
+double PIDController::GetDeltaSetpoint() const {
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+  return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
+}
+
+/**
+ * Returns the current difference of the input from the setpoint.
+ *
+ * @return the current error
+ */
+double PIDController::GetError() const {
+  double setpoint = GetSetpoint();
+  {
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+    return GetContinuousError(setpoint - m_pidInput->PIDGet());
+  }
+}
+
+/**
+ * Sets what type of input the PID controller will use.
+ */
+void PIDController::SetPIDSourceType(PIDSourceType pidSource) {
+  m_pidInput->SetPIDSourceType(pidSource);
+}
+
+/**
+ * Returns the type of input the PID controller is using.
+ *
+ * @return the PID controller input type
+ */
+PIDSourceType PIDController::GetPIDSourceType() const {
+  return m_pidInput->GetPIDSourceType();
+}
+
+/**
+ * Returns the current average of the error over the past few iterations.
+ *
+ * You can specify the number of iterations to average with SetToleranceBuffer()
+ * (defaults to 1). This is the same value that is used for OnTarget().
+ *
+ * @return the average error
+ */
+double PIDController::GetAvgError() const {
+  double avgError = 0;
+  {
+    std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+    // Don't divide by zero.
+    if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
+  }
+  return avgError;
+}
+
+/**
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ *
+ * @param percent percentage error which is tolerable
+ */
+void PIDController::SetTolerance(double percent) {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  m_toleranceType = kPercentTolerance;
+  m_tolerance = percent;
+}
+
+/**
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ *
+ * @param absTolerance absolute error which is tolerable
+ */
+void PIDController::SetAbsoluteTolerance(double absTolerance) {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  m_toleranceType = kAbsoluteTolerance;
+  m_tolerance = absTolerance;
+}
+
+/**
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ *
+ * @param percent percentage error which is tolerable
+ */
+void PIDController::SetPercentTolerance(double percent) {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  m_toleranceType = kPercentTolerance;
+  m_tolerance = percent;
+}
+
+/**
+ * Set the number of previous error samples to average for tolerancing.
+ *
+ * When determining whether a mechanism is on target, the user may want to use a
+ * rolling average of previous measurements instead of a precise position or
+ * velocity. This is useful for noisy sensors which return a few erroneous
+ * measurements when the mechanism is on target. However, the mechanism will
+ * not register as on target for at least the specified bufLength cycles.
+ * @param bufLength Number of previous cycles to average. Defaults to 1.
+ */
+void PIDController::SetToleranceBuffer(int bufLength) {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  m_bufLength = bufLength;
+
+  // Cut the buffer down to size if needed.
+  while (m_buf.size() > bufLength) {
+    m_bufTotal -= m_buf.front();
+    m_buf.pop();
+  }
+}
+
+/**
+ * Return true if the error is within the percentage of the total input range,
+ * determined by SetTolerance.
+ *
+ * This asssumes that the maximum and minimum input were set using SetInput.
+ * Currently this just reports on target as the actual value passes through the
+ * setpoint. Ideally it should be based on being within the tolerance for some
+ * period of time.
+ */
+bool PIDController::OnTarget() const {
+  std::lock_guard<priority_recursive_mutex> sync(m_mutex);
+  if (m_buf.size() == 0) return false;
+  double error = GetError();
+  switch (m_toleranceType) {
+    case kPercentTolerance:
+      return std::fabs(error) <
+             m_tolerance / 100 * (m_maximumInput - m_minimumInput);
+      break;
+    case kAbsoluteTolerance:
+      return std::fabs(error) < m_tolerance;
+      break;
+    case kNoTolerance:  // TODO: this case needs an error
+      return false;
+  }
+  return false;
+}
+
+/**
+ * Begin running the PIDController.
+ */
+void PIDController::Enable() {
+  {
+    std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+    m_enabled = true;
+  }
+
+  if (m_table != nullptr) {
+    m_table->PutBoolean("enabled", true);
+  }
+}
+
+/**
+ * Stop running the PIDController, this sets the output to zero before stopping.
+ */
+void PIDController::Disable() {
+  {
+    std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+    m_pidOutput->PIDWrite(0);
+    m_enabled = false;
+  }
+
+  if (m_table != nullptr) {
+    m_table->PutBoolean("enabled", false);
+  }
+}
+
+/**
+ * Return true if PIDController is enabled.
+ */
+bool PIDController::IsEnabled() const {
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  return m_enabled;
+}
+
+/**
+ * Reset the previous error,, the integral term, and disable the controller.
+ */
+void PIDController::Reset() {
+  Disable();
+
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
+  m_prevError = 0;
+  m_totalError = 0;
+  m_result = 0;
+}
+
+std::string PIDController::GetSmartDashboardType() const {
+  return "PIDController";
+}
+
+void PIDController::InitTable(std::shared_ptr<ITable> table) {
+  if (m_table != nullptr) m_table->RemoveTableListener(this);
+  m_table = table;
+  if (m_table != nullptr) {
+    m_table->PutNumber(kP, GetP());
+    m_table->PutNumber(kI, GetI());
+    m_table->PutNumber(kD, GetD());
+    m_table->PutNumber(kF, GetF());
+    m_table->PutNumber(kSetpoint, GetSetpoint());
+    m_table->PutBoolean(kEnabled, IsEnabled());
+    m_table->AddTableListener(this, false);
+  }
+}
+
+/**
+ * Wraps error around for continuous inputs. The original error is returned if
+ * continuous mode is disabled. This is an unsynchronized function.
+ *
+ * @param error The current error of the PID controller.
+ * @return Error for continuous inputs.
+ */
+double PIDController::GetContinuousError(double error) const {
+  if (m_continuous) {
+    if (std::fabs(error) > (m_maximumInput - m_minimumInput) / 2) {
+      if (error > 0) {
+        return error - (m_maximumInput - m_minimumInput);
+      } else {
+        return error + (m_maximumInput - m_minimumInput);
+      }
+    }
+  }
+
+  return error;
+}
+
+std::shared_ptr<ITable> PIDController::GetTable() const { return m_table; }
+
+void PIDController::ValueChanged(ITable* source, llvm::StringRef key,
+                                 std::shared_ptr<nt::Value> value, bool isNew) {
+  if (key == kP || key == kI || key == kD || key == kF) {
+    if (m_P != m_table->GetNumber(kP, 0.0) ||
+        m_I != m_table->GetNumber(kI, 0.0) ||
+        m_D != m_table->GetNumber(kD, 0.0) ||
+        m_F != m_table->GetNumber(kF, 0.0)) {
+      SetPID(m_table->GetNumber(kP, 0.0), m_table->GetNumber(kI, 0.0),
+             m_table->GetNumber(kD, 0.0), m_table->GetNumber(kF, 0.0));
+    }
+  } else if (key == kSetpoint && value->IsDouble() &&
+             m_setpoint != value->GetDouble()) {
+    SetSetpoint(value->GetDouble());
+  } else if (key == kEnabled && value->IsBoolean() &&
+             m_enabled != value->GetBoolean()) {
+    if (value->GetBoolean()) {
+      Enable();
+    } else {
+      Disable();
+    }
+  }
+}
+
+void PIDController::UpdateTable() {}
+
+void PIDController::StartLiveWindowMode() { Disable(); }
+
+void PIDController::StopLiveWindowMode() {}
diff --git a/wpilibc/sim/src/PWM.cpp b/wpilibc/sim/src/PWM.cpp
new file mode 100644
index 0000000..efdaae7
--- /dev/null
+++ b/wpilibc/sim/src/PWM.cpp
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "PWM.h"
+
+#include <sstream>
+
+#include "Utility.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const double PWM::kDefaultPwmPeriod = 5.05;
+const double PWM::kDefaultPwmCenter = 1.5;
+const int PWM::kDefaultPwmStepsDown = 1000;
+const int PWM::kPwmDisabled = 0;
+
+/**
+ * Allocate a PWM given a channel number.
+ *
+ * Checks channel value range and allocates the appropriate channel.
+ * The allocation is only done to help users ensure that they don't double
+ * assign channels.
+ *
+ * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP
+ *                port
+ */
+PWM::PWM(int channel) {
+  std::stringstream ss;
+
+  if (!CheckPWMChannel(channel)) {
+    ss << "PWM Channel " << channel;
+    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, ss.str());
+    return;
+  }
+
+  ss << "pwm/" << channel;
+  impl = new SimContinuousOutput(ss.str());
+  m_channel = channel;
+  m_eliminateDeadband = false;
+
+  m_centerPwm = kPwmDisabled;  // In simulation, the same thing.
+}
+
+PWM::~PWM() {
+  if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Optionally eliminate the deadband from a speed controller.
+ *
+ * @param eliminateDeadband If true, set the motor curve on the Jaguar to
+ *                          eliminate the deadband in the middle of the range.
+ *                          Otherwise, keep the full range without modifying
+ *                          any values.
+ */
+void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
+  m_eliminateDeadband = eliminateDeadband;
+}
+
+/**
+ * Set the bounds on the PWM values.
+ *
+ * This sets the bounds on the PWM values for a particular each type of
+ * controller. The values determine the upper and lower speeds as well as the
+ * deadband bracket.
+ *
+ * @param max         The Minimum pwm value
+ * @param deadbandMax The high end of the deadband range
+ * @param center      The center speed (off)
+ * @param deadbandMin The low end of the deadband range
+ * @param min         The minimum pwm value
+ */
+void PWM::SetBounds(int max, int deadbandMax, int center, int deadbandMin,
+                    int min) {
+  // Nothing to do in simulation.
+}
+
+/**
+ * Set the bounds on the PWM pulse widths.
+ *
+ * This sets the bounds on the PWM values for a particular type of controller.
+ * The values determine the upper and lower speeds as well as the deadband
+ * bracket.
+ *
+ * @param max         The max PWM pulse width in ms
+ * @param deadbandMax The high end of the deadband range pulse width in ms
+ * @param center      The center (off) pulse width in ms
+ * @param deadbandMin The low end of the deadband pulse width in ms
+ * @param min         The minimum pulse width in ms
+ */
+void PWM::SetBounds(double max, double deadbandMax, double center,
+                    double deadbandMin, double min) {
+  // Nothing to do in simulation.
+}
+
+/**
+ * Set the PWM value based on a position.
+ *
+ * This is intended to be used by servos.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @param pos The position to set the servo between 0.0 and 1.0.
+ */
+void PWM::SetPosition(double pos) {
+  if (pos < 0.0) {
+    pos = 0.0;
+  } else if (pos > 1.0) {
+    pos = 1.0;
+  }
+
+  impl->Set(pos);
+}
+
+/**
+ * Get the PWM value in terms of a position.
+ *
+ * This is intended to be used by servos.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @return The position the servo is set to between 0.0 and 1.0.
+ */
+double PWM::GetPosition() const {
+  double value = impl->Get();
+  if (value < 0.0) {
+    return 0.0;
+  } else if (value > 1.0) {
+    return 1.0;
+  } else {
+    return value;
+  }
+}
+
+/**
+ * Set the PWM value based on a speed.
+ *
+ * This is intended to be used by speed controllers.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetCenterPwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @param speed The speed to set the speed controller between -1.0 and 1.0.
+ */
+void PWM::SetSpeed(double speed) {
+  // clamp speed to be in the range 1.0 >= speed >= -1.0
+  if (speed < -1.0) {
+    speed = -1.0;
+  } else if (speed > 1.0) {
+    speed = 1.0;
+  }
+
+  impl->Set(speed);
+}
+
+/**
+ * Get the PWM value in terms of speed.
+ *
+ * This is intended to be used by speed controllers.
+ *
+ * @pre SetMaxPositivePwm() called.
+ * @pre SetMinPositivePwm() called.
+ * @pre SetMaxNegativePwm() called.
+ * @pre SetMinNegativePwm() called.
+ *
+ * @return The most recently set speed between -1.0 and 1.0.
+ */
+double PWM::GetSpeed() const {
+  double value = impl->Get();
+  if (value > 1.0) {
+    return 1.0;
+  } else if (value < -1.0) {
+    return -1.0;
+  } else {
+    return value;
+  }
+}
+
+/**
+ * Set the PWM value directly to the hardware.
+ *
+ * Write a raw value to a PWM channel.
+ *
+ * @param value Raw PWM value.
+ */
+void PWM::SetRaw(uint16_t value) {
+  wpi_assert(value == kPwmDisabled);
+  impl->Set(0);
+}
+
+/**
+ * Slow down the PWM signal for old devices.
+ *
+ * @param mult The period multiplier to apply to this channel
+ */
+void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
+  // Do nothing in simulation.
+}
+
+void PWM::ValueChanged(ITable* source, llvm::StringRef key,
+                       std::shared_ptr<nt::Value> value, bool isNew) {
+  if (!value->IsDouble()) return;
+  SetSpeed(value->GetDouble());
+}
+
+void PWM::UpdateTable() {
+  if (m_table != nullptr) {
+    m_table->PutNumber("Value", GetSpeed());
+  }
+}
+
+void PWM::StartLiveWindowMode() {
+  SetSpeed(0);
+  if (m_table != nullptr) {
+    m_table->AddTableListener("Value", this, true);
+  }
+}
+
+void PWM::StopLiveWindowMode() {
+  SetSpeed(0);
+  if (m_table != nullptr) {
+    m_table->RemoveTableListener(this);
+  }
+}
+
+std::string PWM::GetSmartDashboardType() const { return "Speed Controller"; }
+
+void PWM::InitTable(std::shared_ptr<ITable> subTable) {
+  m_table = subTable;
+  UpdateTable();
+}
+
+std::shared_ptr<ITable> PWM::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/Relay.cpp b/wpilibc/sim/src/Relay.cpp
new file mode 100644
index 0000000..898612a
--- /dev/null
+++ b/wpilibc/sim/src/Relay.cpp
@@ -0,0 +1,245 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "Relay.h"
+
+#include <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "MotorSafetyHelper.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+/**
+ * Relay constructor given a channel.
+ *
+ * This code initializes the relay and reserves all resources that need to be
+ * locked. Initially the relay is set to both lines at 0v.
+ *
+ * @param channel   The channel number (0-3).
+ * @param direction The direction that the Relay object will control.
+ */
+Relay::Relay(int channel, Relay::Direction direction)
+    : m_channel(channel), m_direction(direction) {
+  std::stringstream ss;
+  if (!SensorBase::CheckRelayChannel(m_channel)) {
+    ss << "Relay Channel " << m_channel;
+    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, ss.str());
+    return;
+  }
+
+  m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+  m_safetyHelper->SetSafetyEnabled(false);
+
+  ss << "relay/" << m_channel;
+  impl = new SimContinuousOutput(ss.str());  // TODO: Allow two different relays
+                                             // (targetting the different halves
+                                             // of a relay) to be combined to
+                                             // control one motor.
+  LiveWindow::GetInstance()->AddActuator("Relay", 1, m_channel, this);
+  go_pos = go_neg = false;
+}
+
+/**
+ * Free the resource associated with a relay.
+ *
+ * The relay channels are set to free and the relay output is turned off.
+ */
+Relay::~Relay() {
+  impl->Set(0);
+  if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the relay state.
+ *
+ * Valid values depend on which directions of the relay are controlled by the
+ * object.
+ *
+ * When set to kBothDirections, the relay can be any of the four states:
+ *    0v-0v, 0v-12v, 12v-0v, 12v-12v
+ *
+ * When set to kForwardOnly or kReverseOnly, you can specify the constant for
+ * the direction or you can simply specify kOff and kOn.  Using only kOff and
+ * kOn is recommended.
+ *
+ * @param value The state to set the relay.
+ */
+void Relay::Set(Relay::Value value) {
+  switch (value) {
+    case kOff:
+      if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+        go_pos = false;
+      }
+      if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+        go_neg = false;
+      }
+      break;
+    case kOn:
+      if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+        go_pos = true;
+      }
+      if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+        go_neg = true;
+      }
+      break;
+    case kForward:
+      if (m_direction == kReverseOnly) {
+        wpi_setWPIError(IncompatibleMode);
+        break;
+      }
+      if (m_direction == kBothDirections || m_direction == kForwardOnly) {
+        go_pos = true;
+      }
+      if (m_direction == kBothDirections) {
+        go_neg = false;
+      }
+      break;
+    case kReverse:
+      if (m_direction == kForwardOnly) {
+        wpi_setWPIError(IncompatibleMode);
+        break;
+      }
+      if (m_direction == kBothDirections) {
+        go_pos = false;
+      }
+      if (m_direction == kBothDirections || m_direction == kReverseOnly) {
+        go_neg = true;
+      }
+      break;
+  }
+  impl->Set((go_pos ? 1 : 0) + (go_neg ? -1 : 0));
+}
+
+/**
+ * Get the Relay State
+ *
+ * Gets the current state of the relay.
+ *
+ * When set to kForwardOnly or kReverseOnly, value is returned as kOn/kOff not
+ * kForward/kReverse (per the recommendation in Set).
+ *
+ * @return The current state of the relay as a Relay::Value
+ */
+Relay::Value Relay::Get() const {
+  // TODO: Don't assume that the go_pos and go_neg fields are correct?
+  if ((go_pos || m_direction == kReverseOnly) &&
+      (go_neg || m_direction == kForwardOnly)) {
+    return kOn;
+  } else if (go_pos) {
+    return kForward;
+  } else if (go_neg) {
+    return kReverse;
+  } else {
+    return kOff;
+  }
+}
+
+int Relay::GetChannel() const { return m_channel; }
+
+/**
+ * Set the expiration time for the Relay object.
+ *
+ * @param timeout The timeout (in seconds) for this relay object
+ */
+void Relay::SetExpiration(double timeout) {
+  m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the relay object.
+ *
+ * @return The expiration time value.
+ */
+double Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
+
+/**
+ * Check if the relay object is currently alive or stopped due to a timeout.
+ *
+ * @return a bool value that is true if the motor has NOT timed out and should
+ *         still be running.
+ */
+bool Relay::IsAlive() const { return m_safetyHelper->IsAlive(); }
+
+/**
+ * Stop the motor associated with this PWM object.
+ *
+ * This is called by the MotorSafetyHelper object when it has a timeout for this
+ * relay and needs to stop it from running.
+ */
+void Relay::StopMotor() { Set(kOff); }
+
+/**
+ * Enable/disable motor safety for this device
+ *
+ * Turn on and off the motor safety option for this relay object.
+ *
+ * @param enabled True if motor safety is enforced for this object
+ */
+void Relay::SetSafetyEnabled(bool enabled) {
+  m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+/**
+ * Check if motor safety is enabled for this object.
+ *
+ * @return True if motor safety is enforced for this object
+ */
+bool Relay::IsSafetyEnabled() const {
+  return m_safetyHelper->IsSafetyEnabled();
+}
+
+void Relay::GetDescription(std::ostringstream& desc) const {
+  desc << "Relay " << GetChannel();
+}
+
+void Relay::ValueChanged(ITable* source, llvm::StringRef key,
+                         std::shared_ptr<nt::Value> value, bool isNew) {
+  if (!value->IsString()) return;
+  if (value->GetString() == "Off")
+    Set(kOff);
+  else if (value->GetString() == "Forward")
+    Set(kForward);
+  else if (value->GetString() == "Reverse")
+    Set(kReverse);
+}
+
+void Relay::UpdateTable() {
+  if (m_table != nullptr) {
+    if (Get() == kOn) {
+      m_table->PutString("Value", "On");
+    } else if (Get() == kForward) {
+      m_table->PutString("Value", "Forward");
+    } else if (Get() == kReverse) {
+      m_table->PutString("Value", "Reverse");
+    } else {
+      m_table->PutString("Value", "Off");
+    }
+  }
+}
+
+void Relay::StartLiveWindowMode() {
+  if (m_table != nullptr) {
+    m_table->AddTableListener("Value", this, true);
+  }
+}
+
+void Relay::StopLiveWindowMode() {
+  if (m_table != nullptr) {
+    m_table->RemoveTableListener(this);
+  }
+}
+
+std::string Relay::GetSmartDashboardType() const { return "Relay"; }
+
+void Relay::InitTable(std::shared_ptr<ITable> subTable) {
+  m_table = subTable;
+  UpdateTable();
+}
+
+std::shared_ptr<ITable> Relay::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/RobotBase.cpp b/wpilibc/sim/src/RobotBase.cpp
new file mode 100644
index 0000000..82a2e65
--- /dev/null
+++ b/wpilibc/sim/src/RobotBase.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "RobotBase.h"
+
+#include "RobotState.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include "Utility.h"
+
+using namespace frc;
+
+/**
+ * Constructor for a generic robot program.
+ *
+ * User code should be placed in the constuctor that runs before the Autonomous
+ * or Operator Control period starts. The constructor will run to completion
+ * before Autonomous is entered.
+ *
+ * This must be used to ensure that the communications code starts. In the
+ * future it would be nice to put this code into it's own task that loads on
+ * boot so ensure that it runs.
+ */
+RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
+  RobotState::SetImplementation(DriverStation::GetInstance());
+  SmartDashboard::init();
+  time_sub = MainNode::Subscribe("~/time", &wpilib::internal::time_callback);
+}
+
+/**
+ * Determine if the Robot is currently enabled.
+ *
+ * @return True if the Robot is currently enabled by the field controls.
+ */
+bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
+
+/**
+ * Determine if the Robot is currently disabled.
+ *
+ * @return True if the Robot is currently disabled by the field controls.
+ */
+bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
+
+/**
+ * Determine if the robot is currently in Autnomous mode.
+ *
+ * @return True if the robot is currently operating Autonomously as determined
+ *         by the field controls.
+ */
+bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
+
+/**
+ * Determine if the robot is currently in Operator Control mode.
+ *
+ * @return True if the robot is currently operating in Tele-Op mode as
+ *         determined by the field controls.
+ */
+bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
+
+/**
+ * Determine if the robot is currently in Test mode.
+ *
+ * @return True if the robot is currently running tests as determined by the
+ *         field controls.
+ */
+bool RobotBase::IsTest() const { return m_ds.IsTest(); }
diff --git a/wpilibc/sim/src/RobotDrive.cpp b/wpilibc/sim/src/RobotDrive.cpp
new file mode 100644
index 0000000..8974465
--- /dev/null
+++ b/wpilibc/sim/src/RobotDrive.cpp
@@ -0,0 +1,728 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "RobotDrive.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include "GenericHID.h"
+#include "Joystick.h"
+#include "Talon.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const int RobotDrive::kMaxNumberOfMotors;
+
+static auto make_shared_nodelete(SpeedController* ptr) {
+  return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
+}
+
+/*
+ * Driving functions.
+ *
+ * These functions provide an interface to multiple motors that is used for C
+ * programming.
+ * The Drive(speed, direction) function is the main part of the set that makes
+ * it easy to set speeds and direction independently in one call.
+ */
+
+/**
+ * Common function to initialize all the robot drive constructors.
+ * Create a motor safety object (the real reason for the common code) and
+ * initialize all the motor assignments. The default timeout is set for the
+ * robot drive.
+ */
+void RobotDrive::InitRobotDrive() {
+  // FIXME: m_safetyHelper = new MotorSafetyHelper(this);
+  // FIXME: m_safetyHelper->SetSafetyEnabled(true);
+}
+
+/**
+ * Constructor for RobotDrive with 2 motors specified with channel numbers.
+ *
+ * Set up parameters for a two wheel drive system where the
+ * left and right motor pwm channels are specified in the call.
+ * This call assumes Talons for controlling the motors.
+ *
+ * @param leftMotorChannel  The PWM channel number that drives the left motor.
+ * @param rightMotorChannel The PWM channel number that drives the right motor.
+ */
+RobotDrive::RobotDrive(int leftMotorChannel, int rightMotorChannel) {
+  InitRobotDrive();
+  m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
+  m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
+  SetLeftRightMotorOutputs(0.0, 0.0);
+  m_deleteSpeedControllers = true;
+}
+
+/**
+ * Constructor for RobotDrive with 4 motors specified with channel numbers.
+ *
+ * Set up parameters for a four wheel drive system where all four motor
+ * pwm channels are specified in the call.
+ * This call assumes Talons for controlling the motors.
+ *
+ * @param frontLeftMotor  Front left motor channel number
+ * @param rearLeftMotor   Rear Left motor channel number
+ * @param frontRightMotor Front right motor channel number
+ * @param rearRightMotor  Rear Right motor channel number
+ */
+RobotDrive::RobotDrive(int frontLeftMotor, int rearLeftMotor,
+                       int frontRightMotor, int rearRightMotor) {
+  InitRobotDrive();
+  m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
+  m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
+  m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
+  m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
+  SetLeftRightMotorOutputs(0.0, 0.0);
+  m_deleteSpeedControllers = true;
+}
+
+/**
+ * Constructor for RobotDrive with 2 motors specified as SpeedController
+ * objects.
+ *
+ * The SpeedController version of the constructor enables programs to use the
+ * RobotDrive classes with subclasses of the SpeedController objects, for
+ * example, versions with ramping or reshaping of the curve to suit motor bias
+ * or deadband elimination.
+ *
+ * @param leftMotor  The left SpeedController object used to drive the robot.
+ * @param rightMotor the right SpeedController object used to drive the robot.
+ */
+RobotDrive::RobotDrive(SpeedController* leftMotor,
+                       SpeedController* rightMotor) {
+  InitRobotDrive();
+  if (leftMotor == nullptr || rightMotor == nullptr) {
+    wpi_setWPIError(NullParameter);
+    m_rearLeftMotor = m_rearRightMotor = nullptr;
+    return;
+  }
+  m_rearLeftMotor = make_shared_nodelete(leftMotor);
+  m_rearRightMotor = make_shared_nodelete(rightMotor);
+}
+
+// TODO: Change to rvalue references & move syntax.
+RobotDrive::RobotDrive(SpeedController& leftMotor,
+                       SpeedController& rightMotor) {
+  InitRobotDrive();
+  m_rearLeftMotor = make_shared_nodelete(&leftMotor);
+  m_rearRightMotor = make_shared_nodelete(&rightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> leftMotor,
+                       std::shared_ptr<SpeedController> rightMotor) {
+  InitRobotDrive();
+  if (leftMotor == nullptr || rightMotor == nullptr) {
+    wpi_setWPIError(NullParameter);
+    m_rearLeftMotor = m_rearRightMotor = nullptr;
+    return;
+  }
+  m_rearLeftMotor = leftMotor;
+  m_rearRightMotor = rightMotor;
+}
+
+/**
+ * Constructor for RobotDrive with 4 motors specified as SpeedController
+ * objects.
+ *
+ * Speed controller input version of RobotDrive (see previous comments).
+ *
+ * @param rearLeftMotor   The back left SpeedController object used to drive the
+ *                        robot.
+ * @param frontLeftMotor  The front left SpeedController object used to drive
+ *                        the robot
+ * @param rearRightMotor  The back right SpeedController object used to drive
+ *                        the robot.
+ * @param frontRightMotor The front right SpeedController object used to drive
+ *                        the robot.
+ */
+RobotDrive::RobotDrive(SpeedController* frontLeftMotor,
+                       SpeedController* rearLeftMotor,
+                       SpeedController* frontRightMotor,
+                       SpeedController* rearRightMotor) {
+  InitRobotDrive();
+  if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+      frontRightMotor == nullptr || rearRightMotor == nullptr) {
+    wpi_setWPIError(NullParameter);
+    return;
+  }
+  m_frontLeftMotor = make_shared_nodelete(frontLeftMotor);
+  m_rearLeftMotor = make_shared_nodelete(rearLeftMotor);
+  m_frontRightMotor = make_shared_nodelete(frontRightMotor);
+  m_rearRightMotor = make_shared_nodelete(rearRightMotor);
+}
+
+RobotDrive::RobotDrive(SpeedController& frontLeftMotor,
+                       SpeedController& rearLeftMotor,
+                       SpeedController& frontRightMotor,
+                       SpeedController& rearRightMotor) {
+  InitRobotDrive();
+  m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor);
+  m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor);
+  m_frontRightMotor = make_shared_nodelete(&frontRightMotor);
+  m_rearRightMotor = make_shared_nodelete(&rearRightMotor);
+}
+
+RobotDrive::RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
+                       std::shared_ptr<SpeedController> rearLeftMotor,
+                       std::shared_ptr<SpeedController> frontRightMotor,
+                       std::shared_ptr<SpeedController> rearRightMotor) {
+  InitRobotDrive();
+  if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
+      frontRightMotor == nullptr || rearRightMotor == nullptr) {
+    wpi_setWPIError(NullParameter);
+    return;
+  }
+  m_frontLeftMotor = frontLeftMotor;
+  m_rearLeftMotor = rearLeftMotor;
+  m_frontRightMotor = frontRightMotor;
+  m_rearRightMotor = rearRightMotor;
+}
+
+/**
+ * Drive the motors at "outputMagnitude" and "curve".
+ * Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0 represents
+ * stopped and not turning. curve < 0 will turn left and curve > 0 will turn
+ * right.
+ *
+ * The algorithm for steering provides a constant turn radius for any normal
+ * speed range, both forward and backward. Increasing m_sensitivity causes
+ * sharper turns for fixed values of curve.
+ *
+ * This function will most likely be used in an autonomous routine.
+ *
+ * @param outputMagnitude The speed setting for the outside wheel in a turn,
+ *                        forward or backwards, +1 to -1.
+ * @param curve           The rate of turn, constant for different forward
+ *                        speeds. Set curve < 0 for left turn or curve > 0 for
+ *                        right turn. Set curve = e^(-r/w) to get a turn radius
+ *                        r for wheelbase w of your robot. Conversely, turn
+ *                        radius r = -ln(curve)*w for a given value of curve and
+ *                        wheelbase w.
+ */
+void RobotDrive::Drive(double outputMagnitude, double curve) {
+  double leftOutput, rightOutput;
+  static bool reported = false;
+  if (!reported) {
+    reported = true;
+  }
+
+  if (curve < 0) {
+    double value = std::log(-curve);
+    double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+    if (ratio == 0) ratio = .0000000001;
+    leftOutput = outputMagnitude / ratio;
+    rightOutput = outputMagnitude;
+  } else if (curve > 0) {
+    double value = std::log(curve);
+    double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+    if (ratio == 0) ratio = .0000000001;
+    leftOutput = outputMagnitude;
+    rightOutput = outputMagnitude / ratio;
+  } else {
+    leftOutput = outputMagnitude;
+    rightOutput = outputMagnitude;
+  }
+  SetLeftRightMotorOutputs(leftOutput, rightOutput);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ *
+ * Drive the robot using two joystick inputs. The Y-axis will be selected from
+ * each Joystick object.
+ *
+ * @param leftStick  The joystick to control the left side of the robot.
+ * @param rightStick The joystick to control the right side of the robot.
+ */
+void RobotDrive::TankDrive(GenericHID* leftStick, GenericHID* rightStick,
+                           bool squaredInputs) {
+  if (leftStick == nullptr || rightStick == nullptr) {
+    wpi_setWPIError(NullParameter);
+    return;
+  }
+  TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID& leftStick, GenericHID& rightStick,
+                           bool squaredInputs) {
+  TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ *
+ * This function lets you pick the axis to be used on each Joystick object for
+ * the left and right sides of the robot.
+ *
+ * @param leftStick  The Joystick object to use for the left side of the robot.
+ * @param leftAxis   The axis to select on the left side Joystick object.
+ * @param rightStick The Joystick object to use for the right side of the robot.
+ * @param rightAxis  The axis to select on the right side Joystick object.
+ */
+void RobotDrive::TankDrive(GenericHID* leftStick, int leftAxis,
+                           GenericHID* rightStick, int rightAxis,
+                           bool squaredInputs) {
+  if (leftStick == nullptr || rightStick == nullptr) {
+    wpi_setWPIError(NullParameter);
+    return;
+  }
+  TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis),
+            squaredInputs);
+}
+
+void RobotDrive::TankDrive(GenericHID& leftStick, int leftAxis,
+                           GenericHID& rightStick, int rightAxis,
+                           bool squaredInputs) {
+  TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis),
+            squaredInputs);
+}
+
+/**
+ * Provide tank steering using the stored robot configuration.
+ *
+ * This function lets you directly provide joystick values from any source.
+ *
+ * @param leftValue  The value of the left stick.
+ * @param rightValue The value of the right stick.
+ */
+void RobotDrive::TankDrive(double leftValue, double rightValue,
+                           bool squaredInputs) {
+  static bool reported = false;
+  if (!reported) {
+    reported = true;
+  }
+
+  // square the inputs (while preserving the sign) to increase fine control
+  // while permitting full power
+  leftValue = Limit(leftValue);
+  rightValue = Limit(rightValue);
+  if (squaredInputs) {
+    if (leftValue >= 0.0) {
+      leftValue = (leftValue * leftValue);
+    } else {
+      leftValue = -(leftValue * leftValue);
+    }
+    if (rightValue >= 0.0) {
+      rightValue = (rightValue * rightValue);
+    } else {
+      rightValue = -(rightValue * rightValue);
+    }
+  }
+
+  SetLeftRightMotorOutputs(leftValue, rightValue);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * Given a single Joystick, the class assumes the Y axis for the move value and
+ * the X axis for the rotate value.
+ * (Should add more information here regarding the way that arcade drive works.)
+ *
+ * @param stick         The joystick to use for Arcade single-stick driving.
+ *                      The Y-axis will be selected for forwards/backwards and
+ *                      the X-axis will be selected for rotation rate.
+ * @param squaredInputs If true, the sensitivity will be increased for small
+ *                      values
+ */
+void RobotDrive::ArcadeDrive(GenericHID* stick, bool squaredInputs) {
+  // simply call the full-featured ArcadeDrive with the appropriate values
+  ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * Given a single Joystick, the class assumes the Y axis for the move value and
+ * the X axis for the rotate value.
+ * (Should add more information here regarding the way that arcade drive works.)
+ *
+ * @param stick         The joystick to use for Arcade single-stick driving.
+ *                      The Y-axis will be selected for forwards/backwards and
+ *                      the X-axis will be selected for rotation rate.
+ * @param squaredInputs If true, the sensitivity will be increased for small
+ *                      values
+ */
+void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) {
+  // simply call the full-featured ArcadeDrive with the appropriate values
+  ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * Given two joystick instances and two axis, compute the values to send to
+ * either two or four motors.
+ *
+ * @param moveStick     The Joystick object that represents the forward/backward
+ *                      direction
+ * @param moveAxis      The axis on the moveStick object to use for
+ *                      forwards/backwards (typically Y_AXIS)
+ * @param rotateStick   The Joystick object that represents the rotation value
+ * @param rotateAxis    The axis on the rotation object to use for the rotate
+ *                      right/left (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true increases the sensitivity
+ *                      at lower speeds
+ */
+void RobotDrive::ArcadeDrive(GenericHID* moveStick, int moveAxis,
+                             GenericHID* rotateStick, int rotateAxis,
+                             bool squaredInputs) {
+  double moveValue = moveStick->GetRawAxis(moveAxis);
+  double rotateValue = rotateStick->GetRawAxis(rotateAxis);
+
+  ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * Given two joystick instances and two axis, compute the values to send to
+ * either two or four motors.
+ *
+ * @param moveStick     The Joystick object that represents the forward/backward
+ *                      direction
+ * @param moveAxis      The axis on the moveStick object to use for
+ *                      forwards/backwards (typically Y_AXIS)
+ * @param rotateStick   The Joystick object that represents the rotation value
+ * @param rotateAxis    The axis on the rotation object to use for the rotate
+ *                      right/left (typically X_AXIS)
+ * @param squaredInputs Setting this parameter to true increases the sensitivity
+ *                      at lower speeds
+ */
+void RobotDrive::ArcadeDrive(GenericHID& moveStick, int moveAxis,
+                             GenericHID& rotateStick, int rotateAxis,
+                             bool squaredInputs) {
+  double moveValue = moveStick.GetRawAxis(moveAxis);
+  double rotateValue = rotateStick.GetRawAxis(rotateAxis);
+
+  ArcadeDrive(moveValue, rotateValue, squaredInputs);
+}
+
+/**
+ * Arcade drive implements single stick driving.
+ *
+ * This function lets you directly provide joystick values from any source.
+ *
+ * @param moveValue     The value to use for fowards/backwards
+ * @param rotateValue   The value to use for the rotate right/left
+ * @param squaredInputs If set, increases the sensitivity at low speeds
+ */
+void RobotDrive::ArcadeDrive(double moveValue, double rotateValue,
+                             bool squaredInputs) {
+  static bool reported = false;
+  if (!reported) {
+    reported = true;
+  }
+
+  // local variables to hold the computed PWM values for the motors
+  double leftMotorOutput;
+  double rightMotorOutput;
+
+  moveValue = Limit(moveValue);
+  rotateValue = Limit(rotateValue);
+
+  if (squaredInputs) {
+    // square the inputs (while preserving the sign) to increase fine control
+    // while permitting full power
+    if (moveValue >= 0.0) {
+      moveValue = (moveValue * moveValue);
+    } else {
+      moveValue = -(moveValue * moveValue);
+    }
+    if (rotateValue >= 0.0) {
+      rotateValue = (rotateValue * rotateValue);
+    } else {
+      rotateValue = -(rotateValue * rotateValue);
+    }
+  }
+
+  if (moveValue > 0.0) {
+    if (rotateValue > 0.0) {
+      leftMotorOutput = moveValue - rotateValue;
+      rightMotorOutput = std::max(moveValue, rotateValue);
+    } else {
+      leftMotorOutput = std::max(moveValue, -rotateValue);
+      rightMotorOutput = moveValue + rotateValue;
+    }
+  } else {
+    if (rotateValue > 0.0) {
+      leftMotorOutput = -std::max(-moveValue, rotateValue);
+      rightMotorOutput = moveValue + rotateValue;
+    } else {
+      leftMotorOutput = moveValue - rotateValue;
+      rightMotorOutput = -std::max(-moveValue, -rotateValue);
+    }
+  }
+  SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
+}
+
+/**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * A method for driving with Mecanum wheeled robots. There are 4 wheels
+ * on the robot, arranged so that the front and back wheels are toed in 45
+ * degrees. When looking at the wheels from the top, the roller axles should
+ * form an X across the robot.
+ *
+ * This is designed to be directly driven by joystick axes.
+ *
+ * @param x         The speed that the robot should drive in the X direction.
+ *                  [-1.0..1.0]
+ * @param y         The speed that the robot should drive in the Y direction.
+ *                  This input is inverted to match the forward == -1.0 that
+ *                  joysticks produce. [-1.0..1.0]
+ * @param rotation  The rate of rotation for the robot that is completely
+ *                  independent of the translation. [-1.0..1.0]
+ * @param gyroAngle The current angle reading from the gyro.  Use this to
+ *                  implement field-oriented controls.
+ */
+void RobotDrive::MecanumDrive_Cartesian(double x, double y, double rotation,
+                                        double gyroAngle) {
+  static bool reported = false;
+  if (!reported) {
+    reported = true;
+  }
+
+  double xIn = x;
+  double yIn = y;
+  // Negate y for the joystick.
+  yIn = -yIn;
+  // Compenstate for gyro angle.
+  RotateVector(xIn, yIn, gyroAngle);
+
+  double wheelSpeeds[kMaxNumberOfMotors];
+  wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
+  wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
+  wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
+  wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
+
+  Normalize(wheelSpeeds);
+
+  m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] *
+                        m_invertedMotors[kFrontLeftMotor] * m_maxOutput);
+  m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] *
+                         m_invertedMotors[kFrontRightMotor] * m_maxOutput);
+  m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] *
+                       m_invertedMotors[kRearLeftMotor] * m_maxOutput);
+  m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] *
+                        m_invertedMotors[kRearRightMotor] * m_maxOutput);
+
+  // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Drive method for Mecanum wheeled robots.
+ *
+ * A method for driving with Mecanum wheeled robots. There are 4 wheels
+ * on the robot, arranged so that the front and back wheels are toed in 45
+ * degrees. When looking at the wheels from the top, the roller axles should
+ * form an X across the robot.
+ *
+ * @param magnitude The speed that the robot should drive in a given direction.
+ *                  [-1.0..1.0]
+ * @param direction The direction the robot should drive in degrees. The
+ *                  direction and maginitute are independent of the rotation
+ *                  rate.
+ * @param rotation  The rate of rotation for the robot that is completely
+ *                  independent of the magnitute or direction. [-1.0..1.0]
+ */
+void RobotDrive::MecanumDrive_Polar(double magnitude, double direction,
+                                    double rotation) {
+  static bool reported = false;
+  if (!reported) {
+    reported = true;
+  }
+
+  // Normalized for full power along the Cartesian axes.
+  magnitude = Limit(magnitude) * std::sqrt(2.0);
+  // The rollers are at 45 degree angles.
+  double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
+  double cosD = std::cos(dirInRad);
+  double sinD = std::sin(dirInRad);
+
+  double wheelSpeeds[kMaxNumberOfMotors];
+  wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
+  wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
+  wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
+  wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
+
+  Normalize(wheelSpeeds);
+
+  m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] *
+                        m_invertedMotors[kFrontLeftMotor] * m_maxOutput);
+  m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] *
+                         m_invertedMotors[kFrontRightMotor] * m_maxOutput);
+  m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] *
+                       m_invertedMotors[kRearLeftMotor] * m_maxOutput);
+  m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] *
+                        m_invertedMotors[kRearRightMotor] * m_maxOutput);
+
+  // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Holonomic Drive method for Mecanum wheeled robots.
+ *
+ * This is an alias to MecanumDrive_Polar() for backward compatability
+ *
+ * @param magnitude The speed that the robot should drive in a given direction.
+ *                  [-1.0..1.0]
+ * @param direction The direction the robot should drive. The direction and
+ *                  magnitude are independent of the rotation rate.
+ * @param rotation  The rate of rotation for the robot that is completely
+ *                  independent of the magnitude or direction.  [-1.0..1.0]
+ */
+void RobotDrive::HolonomicDrive(double magnitude, double direction,
+                                double rotation) {
+  MecanumDrive_Polar(magnitude, direction, rotation);
+}
+
+/**
+ * Set the speed of the right and left motors.
+ *
+ * This is used once an appropriate drive setup function is called such as
+ * TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput"
+ * and includes flipping the direction of one side for opposing motors.
+ *
+ * @param leftOutput  The speed to send to the left side of the robot.
+ * @param rightOutput The speed to send to the right side of the robot.
+ */
+void RobotDrive::SetLeftRightMotorOutputs(double leftOutput,
+                                          double rightOutput) {
+  wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
+
+  if (m_frontLeftMotor != nullptr)
+    m_frontLeftMotor->Set(Limit(leftOutput) *
+                          m_invertedMotors[kFrontLeftMotor] * m_maxOutput);
+  m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] *
+                       m_maxOutput);
+
+  if (m_frontRightMotor != nullptr)
+    m_frontRightMotor->Set(-Limit(rightOutput) *
+                           m_invertedMotors[kFrontRightMotor] * m_maxOutput);
+  m_rearRightMotor->Set(-Limit(rightOutput) *
+                        m_invertedMotors[kRearRightMotor] * m_maxOutput);
+
+  // FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+double RobotDrive::Limit(double num) {
+  if (num > 1.0) {
+    return 1.0;
+  }
+  if (num < -1.0) {
+    return -1.0;
+  }
+  return num;
+}
+
+/**
+ * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+ */
+void RobotDrive::Normalize(double* wheelSpeeds) {
+  double maxMagnitude = std::fabs(wheelSpeeds[0]);
+  int i;
+  for (i = 1; i < kMaxNumberOfMotors; i++) {
+    double temp = std::fabs(wheelSpeeds[i]);
+    if (maxMagnitude < temp) maxMagnitude = temp;
+  }
+  if (maxMagnitude > 1.0) {
+    for (i = 0; i < kMaxNumberOfMotors; i++) {
+      wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+    }
+  }
+}
+
+/**
+ * Rotate a vector in Cartesian space.
+ */
+void RobotDrive::RotateVector(double& x, double& y, double angle) {
+  double cosA = std::cos(angle * (3.14159 / 180.0));
+  double sinA = std::sin(angle * (3.14159 / 180.0));
+  double xOut = x * cosA - y * sinA;
+  double yOut = x * sinA + y * cosA;
+  x = xOut;
+  y = yOut;
+}
+
+/**
+ * Invert a motor direction.
+ *
+ * This is used when a motor should run in the opposite direction as the drive
+ * code would normally run it. Motors that are direct drive would be inverted,
+ * the Drive code assumes that the motors are geared with one reversal.
+ *
+ * @param motor      The motor index to invert.
+ * @param isInverted True if the motor should be inverted when operated.
+ */
+void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) {
+  if (motor < 0 || motor > 3) {
+    wpi_setWPIError(InvalidMotorIndex);
+    return;
+  }
+  m_invertedMotors[motor] = isInverted ? -1 : 1;
+}
+
+/**
+ * Set the turning sensitivity.
+ *
+ * This only impacts the Drive() entry-point.
+ *
+ * @param sensitivity Effectively sets the turning sensitivity (or turn radius
+ *                    for a given value)
+ */
+void RobotDrive::SetSensitivity(double sensitivity) {
+  m_sensitivity = sensitivity;
+}
+
+/**
+ * Configure the scaling factor for using RobotDrive with motor controllers in a
+ * mode other than PercentVbus.
+ *
+ * @param maxOutput Multiplied with the output percentage computed by the drive
+ *                  functions.
+ */
+void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
+
+void RobotDrive::SetExpiration(double timeout) {
+  // FIXME: m_safetyHelper->SetExpiration(timeout);
+}
+
+double RobotDrive::GetExpiration() const {
+  return -1;  // FIXME: return m_safetyHelper->GetExpiration();
+}
+
+bool RobotDrive::IsAlive() const {
+  return true;  // FIXME: m_safetyHelper->IsAlive();
+}
+
+bool RobotDrive::IsSafetyEnabled() const {
+  return false;  // FIXME: return m_safetyHelper->IsSafetyEnabled();
+}
+
+void RobotDrive::SetSafetyEnabled(bool enabled) {
+  // FIXME: m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+void RobotDrive::GetDescription(std::ostringstream& desc) const {
+  desc << "RobotDrive";
+}
+
+void RobotDrive::StopMotor() {
+  if (m_frontLeftMotor != nullptr) m_frontLeftMotor->Disable();
+  if (m_frontRightMotor != nullptr) m_frontRightMotor->Disable();
+  if (m_rearLeftMotor != nullptr) m_rearLeftMotor->Disable();
+  if (m_rearRightMotor != nullptr) m_rearRightMotor->Disable();
+}
diff --git a/wpilibc/sim/src/SafePWM.cpp b/wpilibc/sim/src/SafePWM.cpp
new file mode 100644
index 0000000..4e42958
--- /dev/null
+++ b/wpilibc/sim/src/SafePWM.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "SafePWM.h"
+
+#include <memory>
+#include <sstream>
+
+using namespace frc;
+
+/**
+ * Constructor for a SafePWM object taking a channel number.
+ *
+ * @param channel The PWM channel number (0..19).
+ */
+SafePWM::SafePWM(int channel) : PWM(channel) {
+  m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+  m_safetyHelper->SetSafetyEnabled(false);
+}
+
+/**
+ * Set the expiration time for the PWM object.
+ *
+ * @param timeout The timeout (in seconds) for this motor object
+ */
+void SafePWM::SetExpiration(double timeout) {
+  m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the PWM object.
+ *
+ * @returns The expiration time value.
+ */
+double SafePWM::GetExpiration() const {
+  return m_safetyHelper->GetExpiration();
+}
+
+/**
+ * Check if the PWM object is currently alive or stopped due to a timeout.
+ *
+ * @return a bool value that is true if the motor has NOT timed out and should
+ *         still be running.
+ */
+bool SafePWM::IsAlive() const { return m_safetyHelper->IsAlive(); }
+
+/**
+ * Stop the motor associated with this PWM object.
+ *
+ * This is called by the MotorSafetyHelper object when it has a timeout for this
+ * PWM and needs to stop it from running.
+ */
+void SafePWM::StopMotor() { SetRaw(kPwmDisabled); }
+
+/**
+ * Enable/disable motor safety for this device.
+ *
+ * Turn on and off the motor safety option for this PWM object.
+ *
+ * @param enabled True if motor safety is enforced for this object
+ */
+void SafePWM::SetSafetyEnabled(bool enabled) {
+  m_safetyHelper->SetSafetyEnabled(enabled);
+}
+
+/**
+ * Check if motor safety is enabled for this object.
+ *
+ * @returns True if motor safety is enforced for this object
+ */
+bool SafePWM::IsSafetyEnabled() const {
+  return m_safetyHelper->IsSafetyEnabled();
+}
+
+void SafePWM::GetDescription(std::ostringstream& desc) const {
+  desc << "PWM " << GetChannel();
+}
+
+/**
+ * Feed the MotorSafety timer when setting the speed.
+ *
+ * This method is called by the subclass motor whenever it updates its speed,
+ * thereby reseting the timeout value.
+ *
+ * @param speed Value to pass to the PWM class
+ */
+void SafePWM::SetSpeed(double speed) {
+  PWM::SetSpeed(speed);
+  m_safetyHelper->Feed();
+}
diff --git a/wpilibc/sim/src/SampleRobot.cpp b/wpilibc/sim/src/SampleRobot.cpp
new file mode 100644
index 0000000..7488d24
--- /dev/null
+++ b/wpilibc/sim/src/SampleRobot.cpp
@@ -0,0 +1,145 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "SampleRobot.h"
+
+#include <cstdio>
+
+#include "LiveWindow/LiveWindow.h"
+#include "Timer.h"
+#include "networktables/NetworkTable.h"
+
+#if defined(_UNIX)
+#include <unistd.h>
+#elif defined(_WIN32)
+#include <windows.h>
+void sleep(unsigned int milliseconds) { Sleep(milliseconds); }
+#endif
+
+using namespace frc;
+
+SampleRobot::SampleRobot() : m_robotMainOverridden(true) {}
+
+/**
+ * Robot-wide initialization code should go here.
+ *
+ * Programmers should override this method for default Robot-wide initialization
+ * which will be called each time the robot enters the disabled state.
+ */
+void SampleRobot::RobotInit() {
+  std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Disabled should go here.
+ *
+ * Programmers should override this method to run code that should run while the
+ * field is disabled.
+ */
+void SampleRobot::Disabled() {
+  std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Autonomous should go here.
+ *
+ * Programmers should override this method to run code that should run while the
+ * field is in the autonomous period. This will be called once each time the
+ * robot enters the autonomous state.
+ */
+void SampleRobot::Autonomous() {
+  std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Operator control (tele-operated) code should go here.
+ *
+ * Programmers should override this method to run code that should run while the
+ * field is in the Operator Control (tele-operated) period. This is called once
+ * each time the robot enters the teleop state.
+ */
+void SampleRobot::OperatorControl() {
+  std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Test program should go here.
+ *
+ * Programmers should override this method to run code that executes while the
+ * robot is in test mode. This will be called once whenever the robot enters
+ * test mode.
+ */
+void SampleRobot::Test() {
+  std::printf("Default %s() method... Override me!\n", __FUNCTION__);
+}
+
+/**
+ * Robot main program for free-form programs.
+ *
+ * This should be overridden by user subclasses if the intent is to not use the
+ * Autonomous() and OperatorControl() methods. In that case, the program is
+ * responsible for sensing when to run the autonomous and operator control
+ * functions in their program.
+ *
+ * This method will be called immediately after the constructor is called. If it
+ * has not been overridden by a user subclass (i.e. the default version runs),
+ * then the Autonomous() and OperatorControl() methods will be called.
+ */
+void SampleRobot::RobotMain() { m_robotMainOverridden = false; }
+
+/**
+ * Start a competition.
+ *
+ * This code needs to track the order of the field starting to ensure that
+ * everything happens in the right order. Repeatedly run the correct method,
+ * either Autonomous or OperatorControl or Test when the robot is enabled.
+ * After running the correct method, wait for some state to change, either the
+ * other mode starts or the robot is disabled. Then go back and wait for the
+ * robot to be enabled again.
+ */
+void SampleRobot::StartCompetition() {
+  LiveWindow* lw = LiveWindow::GetInstance();
+
+  NetworkTable::GetTable("LiveWindow")
+      ->GetSubTable("~STATUS~")
+      ->PutBoolean("LW Enabled", false);
+
+  RobotMain();
+
+  if (!m_robotMainOverridden) {
+    // first and one-time initialization
+    lw->SetEnabled(false);
+    RobotInit();
+
+    while (true) {
+      if (IsDisabled()) {
+        m_ds.InDisabled(true);
+        Disabled();
+        m_ds.InDisabled(false);
+        while (IsDisabled()) sleep(1);  // m_ds.WaitForData();
+      } else if (IsAutonomous()) {
+        m_ds.InAutonomous(true);
+        Autonomous();
+        m_ds.InAutonomous(false);
+        while (IsAutonomous() && IsEnabled()) sleep(1);  // m_ds.WaitForData();
+      } else if (IsTest()) {
+        lw->SetEnabled(true);
+        m_ds.InTest(true);
+        Test();
+        m_ds.InTest(false);
+        while (IsTest() && IsEnabled()) sleep(1);  // m_ds.WaitForData();
+        lw->SetEnabled(false);
+      } else {
+        m_ds.InOperatorControl(true);
+        OperatorControl();
+        m_ds.InOperatorControl(false);
+        while (IsOperatorControl() && IsEnabled())
+          sleep(1);  // m_ds.WaitForData();
+      }
+    }
+  }
+}
diff --git a/wpilibc/sim/src/SensorBase.cpp b/wpilibc/sim/src/SensorBase.cpp
new file mode 100644
index 0000000..cd708ad
--- /dev/null
+++ b/wpilibc/sim/src/SensorBase.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "SensorBase.h"
+
+#include "WPIErrors.h"
+
+using namespace frc;
+
+const int SensorBase::kDigitalChannels;
+const int SensorBase::kAnalogInputs;
+const int SensorBase::kSolenoidChannels;
+const int SensorBase::kSolenoidModules;
+const int SensorBase::kPwmChannels;
+const int SensorBase::kRelayChannels;
+const int SensorBase::kPDPChannels;
+
+/**
+ * Check that the solenoid module number is valid.
+ *
+ * @return Solenoid module number is valid
+ */
+bool SensorBase::CheckSolenoidModule(int moduleNumber) {
+  return moduleNumber >= 0 && moduleNumber < kSolenoidModules;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ *
+ * Verify that the channel number is one of the legal channel numbers. Channel
+ * numbers are 0-based.
+ *
+ * @return Digital channel is valid
+ */
+bool SensorBase::CheckDigitalChannel(int channel) {
+  if (channel >= 0 && channel < kDigitalChannels) return true;
+  return false;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ *
+ * Verify that the channel number is one of the legal channel numbers. Channel
+ * numbers are 0-based.
+ *
+ * @return Relay channel is valid
+ */
+bool SensorBase::CheckRelayChannel(int channel) {
+  if (channel >= 0 && channel < kRelayChannels) return true;
+  return false;
+}
+
+/**
+ * Check that the digital channel number is valid.
+ *
+ * Verify that the channel number is one of the legal channel numbers. Channel
+ * numbers are 0-based.
+ *
+ * @return PWM channel is valid
+ */
+bool SensorBase::CheckPWMChannel(int channel) {
+  if (channel >= 0 && channel < kPwmChannels) return true;
+  return false;
+}
+
+/**
+ * Check that the analog input number is valid.
+ *
+ * Verify that the analog input number is one of the legal channel numbers.
+ * Channel numbers are 0-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogInputChannel(int channel) {
+  if (channel >= 0 && channel < kAnalogInputs) return true;
+  return false;
+}
+
+/**
+ * Check that the analog output number is valid.
+ *
+ * Verify that the analog output number is one of the legal channel numbers.
+ * Channel numbers are 0-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogOutputChannel(int channel) {
+  if (channel >= 0 && channel < kAnalogOutputs) return true;
+  return false;
+}
+
+/**
+ * Verify that the solenoid channel number is within limits.
+ *
+ * @return Solenoid channel is valid
+ */
+bool SensorBase::CheckSolenoidChannel(int channel) {
+  if (channel >= 0 && channel < kSolenoidChannels) return true;
+  return false;
+}
+
+/**
+ * Verify that the power distribution channel number is within limits.
+ *
+ * @return PDP channel is valid
+ */
+bool SensorBase::CheckPDPChannel(int channel) {
+  if (channel >= 0 && channel < kPDPChannels) return true;
+  return false;
+}
diff --git a/wpilibc/sim/src/Solenoid.cpp b/wpilibc/sim/src/Solenoid.cpp
new file mode 100644
index 0000000..ef19ffb
--- /dev/null
+++ b/wpilibc/sim/src/Solenoid.cpp
@@ -0,0 +1,94 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "Solenoid.h"
+
+#include <sstream>
+
+#include "LiveWindow/LiveWindow.h"
+#include "WPIErrors.h"
+#include "simulation/simTime.h"
+
+using namespace frc;
+
+/**
+ * Constructor.
+ *
+ * @param channel The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(int channel) : Solenoid(1, channel) {}
+
+/**
+ * Constructor.
+ *
+ * @param moduleNumber The solenoid module (1 or 2).
+ * @param channel      The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(int moduleNumber, int channel) {
+  std::stringstream ss;
+  ss << "pneumatic/" << moduleNumber << "/" << channel;
+  m_impl = new SimContinuousOutput(ss.str());
+
+  LiveWindow::GetInstance()->AddActuator("Solenoid", moduleNumber, channel,
+                                         this);
+}
+
+Solenoid::~Solenoid() {
+  if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Set the value of a solenoid.
+ *
+ * @param on Turn the solenoid output off or on.
+ */
+void Solenoid::Set(bool on) {
+  m_on = on;
+  m_impl->Set(on ? 1 : -1);
+}
+
+/**
+ * Read the current value of the solenoid.
+ *
+ * @return The current value of the solenoid.
+ */
+bool Solenoid::Get() const { return m_on; }
+
+void Solenoid::ValueChanged(ITable* source, llvm::StringRef key,
+                            std::shared_ptr<nt::Value> value, bool isNew) {
+  if (!value->IsBoolean()) return;
+  Set(value->GetBoolean());
+}
+
+void Solenoid::UpdateTable() {
+  if (m_table != nullptr) {
+    m_table->PutBoolean("Value", Get());
+  }
+}
+
+void Solenoid::StartLiveWindowMode() {
+  Set(false);
+  if (m_table != nullptr) {
+    m_table->AddTableListener("Value", this, true);
+  }
+}
+
+void Solenoid::StopLiveWindowMode() {
+  Set(false);
+  if (m_table != nullptr) {
+    m_table->RemoveTableListener(this);
+  }
+}
+
+std::string Solenoid::GetSmartDashboardType() const { return "Solenoid"; }
+
+void Solenoid::InitTable(std::shared_ptr<ITable> subTable) {
+  m_table = subTable;
+  UpdateTable();
+}
+
+std::shared_ptr<ITable> Solenoid::GetTable() const { return m_table; }
diff --git a/wpilibc/sim/src/Talon.cpp b/wpilibc/sim/src/Talon.cpp
new file mode 100644
index 0000000..652b2ae
--- /dev/null
+++ b/wpilibc/sim/src/Talon.cpp
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "Talon.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+using namespace frc;
+
+/**
+ * @param channel The PWM channel that the Talon is attached to.
+ */
+Talon::Talon(int channel) : SafePWM(channel) {
+  /* Note that the Talon uses the following bounds for PWM values. These values
+   * should work reasonably well for most controllers, but if users experience
+   * issues such as asymmetric behavior around the deadband or inability to
+   * saturate the controller in either direction, calibration is recommended.
+   * The calibration procedure can be found in the Talon User Manual available
+   * from CTRE.
+   *
+   *   - 211 = full "forward"
+   *   - 133 = the "high end" of the deadband range
+   *   - 129 = center of the deadband range (off)
+   *   - 125 = the "low end" of the deadband range
+   *   - 49 = full "reverse"
+   */
+  SetBounds(2.037, 1.539, 1.513, 1.487, .989);
+  SetPeriodMultiplier(kPeriodMultiplier_2X);
+  SetRaw(m_centerPwm);
+
+  LiveWindow::GetInstance()->AddActuator("Talon", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+void Talon::Set(double speed) { SetSpeed(speed); }
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+double Talon::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Talon::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Talon::PIDWrite(double output) { Set(output); }
diff --git a/wpilibc/sim/src/Timer.cpp b/wpilibc/sim/src/Timer.cpp
new file mode 100644
index 0000000..93e33e2
--- /dev/null
+++ b/wpilibc/sim/src/Timer.cpp
@@ -0,0 +1,192 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "Utility.h"
+#include "simulation/simTime.h"
+
+// Internal stuff
+#include "simulation/MainNode.h"
+#include "simulation/SimFloatInput.h"
+namespace wpilib {
+namespace internal {
+double simTime = 0;
+std::condition_variable time_wait;
+std::mutex time_wait_mutex;
+
+void time_callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  simTime = msg->data();
+  time_wait.notify_all();
+}
+}
+}  // namespace wpilib
+
+namespace frc {
+
+/**
+ * Pause the task for a specified time.
+ *
+ * Pause the execution of the program for a specified period of time given in
+ * seconds. Motors will continue to run at their last assigned values, and
+ * sensors will continue to update. Only the task containing the wait will
+ * pause until the wait time is expired.
+ *
+ * @param seconds Length of time to pause, in seconds.
+ */
+void Wait(double seconds) {
+  if (seconds < 0.0) return;
+
+  double start = wpilib::internal::simTime;
+
+  std::unique_lock<std::mutex> lock(wpilib::internal::time_wait_mutex);
+  while ((wpilib::internal::simTime - start) < seconds) {
+    wpilib::internal::time_wait.wait(lock);
+  }
+}
+
+/*
+ * Return the FPGA system clock time in seconds.
+ *
+ * This is deprecated and just forwards to Timer::GetFPGATimestamp().
+ *
+ * @returns Robot running time in seconds.
+ */
+double GetClock() { return Timer::GetFPGATimestamp(); }
+
+/**
+ * @brief Gives real-time clock system time with nanosecond resolution
+ * @return The time, just in case you want the robot to start autonomous at 8pm
+ *         on Saturday (except in simulation).
+ */
+double GetTime() {
+  return Timer::GetFPGATimestamp();  // The epoch starts when Gazebo starts
+}
+
+}  // namespace frc
+
+using namespace frc;
+
+// for compatibility with msvc12--see C2864
+const double Timer::kRolloverTime = (1ll << 32) / 1e6;
+/**
+ * Create a new timer object.
+ *
+ * Create a new timer object and reset the time to zero. The timer is initially
+ * not running and must be started.
+ */
+Timer::Timer() : m_startTime(0.0), m_accumulatedTime(0.0), m_running(false) {
+  // Creates a semaphore to control access to critical regions.
+  // Initially 'open'
+  Reset();
+}
+
+/**
+ * Get the current time from the timer.
+ *
+ * If the clock is running it is derived from the current system clock the
+ * start time stored in the timer class. If the clock is not running, then
+ * return the time when it was last stopped.
+ *
+ * @return Current time value for this timer in seconds
+ */
+double Timer::Get() const {
+  double result;
+  double currentTime = GetFPGATimestamp();
+
+  std::lock_guard<priority_mutex> sync(m_mutex);
+  if (m_running) {
+    // This math won't work if the timer rolled over (71 minutes after boot).
+    // TODO: Check for it and compensate.
+    result = (currentTime - m_startTime) + m_accumulatedTime;
+  } else {
+    result = m_accumulatedTime;
+  }
+
+  return result;
+}
+
+/**
+ * Reset the timer by setting the time to 0.
+ *
+ * Make the timer startTime the current time so new requests will be relative to
+ * now.
+ */
+void Timer::Reset() {
+  std::lock_guard<priority_mutex> sync(m_mutex);
+  m_accumulatedTime = 0;
+  m_startTime = GetFPGATimestamp();
+}
+
+/**
+ * Start the timer running.
+ *
+ * Just set the running flag to true indicating that all time requests should be
+ * relative to the system clock.
+ */
+void Timer::Start() {
+  std::lock_guard<priority_mutex> sync(m_mutex);
+  if (!m_running) {
+    m_startTime = GetFPGATimestamp();
+    m_running = true;
+  }
+}
+
+/**
+ * Stop the timer.
+ *
+ * This computes the time as of now and clears the running flag, causing all
+ * subsequent time requests to be read from the accumulated time rather than
+ * looking at the system clock.
+ */
+void Timer::Stop() {
+  double temp = Get();
+
+  std::lock_guard<priority_mutex> sync(m_mutex);
+  if (m_running) {
+    m_accumulatedTime = temp;
+    m_running = false;
+  }
+}
+
+/**
+ * Check if the period specified has passed and if it has, advance the start
+ * time by that period. This is useful to decide if it's time to do periodic
+ * work without drifting later by the time it took to get around to checking.
+ *
+ * @param period The period to check for (in seconds).
+ * @return If the period has passed.
+ */
+bool Timer::HasPeriodPassed(double period) {
+  if (Get() > period) {
+    std::lock_guard<priority_mutex> sync(m_mutex);
+    // Advance the start time by the period.
+    // Don't set it to the current time... we want to avoid drift.
+    m_startTime += period;
+    return true;
+  }
+  return false;
+}
+
+/*
+ * Return the FPGA system clock time in seconds.
+ *
+ * Return the time from the FPGA hardware clock in seconds since the FPGA
+ * started. Rolls over after 71 minutes.
+ *
+ * @return Robot running time in seconds.
+ */
+double Timer::GetFPGATimestamp() {
+  // FPGA returns the timestamp in microseconds
+  // Call the helper GetFPGATime() in Utility.cpp
+  return wpilib::internal::simTime;
+}
+
+/*
+ * Not in a match.
+ */
+double Timer::GetMatchTime() { return Timer::GetFPGATimestamp(); }
diff --git a/wpilibc/sim/src/Utility.cpp b/wpilibc/sim/src/Utility.cpp
new file mode 100644
index 0000000..97c02de
--- /dev/null
+++ b/wpilibc/sim/src/Utility.cpp
@@ -0,0 +1,216 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. 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 "Utility.h"
+
+#ifndef _WIN32
+#include <cxxabi.h>
+#include <execinfo.h>
+#endif
+
+#include <cstdio>
+#include <cstdlib>
+#include <iostream>
+#include <sstream>
+
+#include "Timer.h"
+#include "llvm/SmallString.h"
+#include "simulation/simTime.h"
+
+using namespace frc;
+
+static bool stackTraceEnabled = false;
+static bool suspendOnAssertEnabled = false;
+
+/**
+ * Enable Stack trace after asserts.
+ */
+void wpi_stackTraceOnAssertEnable(bool enabled) { stackTraceEnabled = enabled; }
+
+/**
+ * Enable suspend on asssert.
+ *
+ * If enabled, the user task will be suspended whenever an assert fails. This
+ * will allow the user to attach to the task with the debugger and examine
+ * variables around the failure.
+ */
+void wpi_suspendOnAssertEnabled(bool enabled) {
+  suspendOnAssertEnabled = enabled;
+}
+
+static void wpi_handleTracing() {
+  // if (stackTraceEnabled)
+  // {
+  //   std::printf("\n-----------<Stack Trace>----------------\n");
+  //   printCurrentStackTrace();
+  // }
+  std::printf("\n");
+}
+
+/**
+ * Assert implementation.
+ * This allows breakpoints to be set on an assert.
+ * The users don't call this, but instead use the wpi_assert macros in
+ * Utility.h.
+ */
+bool wpi_assert_impl(bool conditionValue, llvm::StringRef conditionText,
+                     llvm::StringRef message, llvm::StringRef fileName,
+                     int lineNumber, llvm::StringRef funcName) {
+  if (!conditionValue) {
+    std::stringstream errorStream;
+
+    errorStream << "Assertion \"" << conditionText << "\" ";
+    errorStream << "on line " << lineNumber << " ";
+
+    llvm::SmallString<128> fileTemp;
+    errorStream << "of " << basename(fileName.c_str(fileTemp)) << " ";
+
+    if (message[0] != '\0') {
+      errorStream << "failed: " << message << std::endl;
+    } else {
+      errorStream << "failed." << std::endl;
+    }
+
+    // Print to console and send to remote dashboard
+    std::cout << "\n\n>>>>" << errorStream.str();
+    wpi_handleTracing();
+  }
+
+  return conditionValue;
+}
+
+/**
+ * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl
+ * This should not be called directly; it should only be used by
+ * wpi_assertEqual_impl and wpi_assertNotEqual_impl.
+ */
+void wpi_assertEqual_common_impl(int valueA, int valueB,
+                                 llvm::StringRef equalityType,
+                                 llvm::StringRef message,
+                                 llvm::StringRef fileName, int lineNumber,
+                                 llvm::StringRef funcName) {
+  // Error string buffer
+  std::stringstream error;
+
+  // If an error message was specified, include it
+  // Build error string
+  if (message.size() > 0) {
+    error << "Assertion failed: \"" << message << "\", \"" << valueA << "\" "
+          << equalityType << " \"" << valueB << "\" in " << funcName << "() in "
+          << fileName << " at line " << lineNumber << "\n";
+  } else {
+    error << "Assertion failed: \"" << valueA << "\" " << equalityType << " \""
+          << valueB << "\" in " << funcName << "() in " << fileName
+          << " at line " << lineNumber << "\n";
+  }
+
+  // Print to console and send to remote dashboard
+  std::cout << "\n\n>>>>" << error.str();
+
+  wpi_handleTracing();
+}
+
+/**
+ * Assert equal implementation.
+ * This determines whether the two given integers are equal. If not,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertEqual macros in
+ * Utility.h.
+ */
+bool wpi_assertEqual_impl(int valueA, int valueB, llvm::StringRef message,
+                          llvm::StringRef fileName, int lineNumber,
+                          llvm::StringRef funcName) {
+  if (!(valueA == valueB)) {
+    wpi_assertEqual_common_impl(valueA, valueB, "!=", message, fileName,
+                                lineNumber, funcName);
+  }
+  return valueA == valueB;
+}
+
+/**
+ * Assert not equal implementation.
+ * This determines whether the two given integers are equal. If so,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertNotEqual macros in
+ * Utility.h.
+ */
+bool wpi_assertNotEqual_impl(int valueA, int valueB, llvm::StringRef message,
+                             llvm::StringRef fileName, int lineNumber,
+                             llvm::StringRef funcName) {
+  if (!(valueA != valueB)) {
+    wpi_assertEqual_common_impl(valueA, valueB, "==", message, fileName,
+                                lineNumber, funcName);
+  }
+  return valueA != valueB;
+}
+
+namespace frc {
+
+/**
+ * Read the microsecond-resolution timer on the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA
+ * reset).
+ */
+uint64_t GetFPGATime() { return wpilib::internal::simTime * 1e6; }
+
+// TODO: implement symbol demangling and backtrace on windows
+#if not defined(_WIN32)
+
+/**
+ * Demangle a C++ symbol, used for printing stack traces.
+ */
+static std::string demangle(char const* mangledSymbol) {
+  char buffer[256];
+  size_t length;
+  int32_t status;
+
+  if (std::sscanf(mangledSymbol, "%*[^(]%*[^_]%255[^)+]", buffer)) {
+    char* symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
+
+    if (status == 0) {
+      return symbol;
+    } else {
+      // If the symbol couldn't be demangled, it's probably a C function,
+      // so just return it as-is.
+      return buffer;
+    }
+  }
+
+  // If everything else failed, just return the mangled symbol
+  return mangledSymbol;
+}
+
+/**
+ * Get a stack trace, ignoring the first "offset" symbols.
+ */
+std::string GetStackTrace(int offset) {
+  void* stackTrace[128];
+  int stackSize = backtrace(stackTrace, 128);
+  char** mangledSymbols = backtrace_symbols(stackTrace, stackSize);
+  std::stringstream trace;
+
+  for (int i = offset; i < stackSize; i++) {
+    // Only print recursive functions once in a row.
+    if (i == 0 || stackTrace[i] != stackTrace[i - 1]) {
+      trace << "\tat " << demangle(mangledSymbols[i]) << std::endl;
+    }
+  }
+
+  std::free(mangledSymbols);
+
+  return trace.str();
+}
+
+#else
+static std::string demangle(char const* mangledSymbol) {
+  return "no demangling on windows";
+}
+std::string GetStackTrace(int offset) { return "no stack trace on windows"; }
+#endif
+
+}  // namespace frc
diff --git a/wpilibc/sim/src/Victor.cpp b/wpilibc/sim/src/Victor.cpp
new file mode 100644
index 0000000..ec61719
--- /dev/null
+++ b/wpilibc/sim/src/Victor.cpp
@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2017. 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 "Victor.h"
+
+#include "LiveWindow/LiveWindow.h"
+
+using namespace frc;
+
+/**
+ * @param channel The PWM channel that the Victor is attached to.
+ */
+Victor::Victor(int channel) : SafePWM(channel) {
+  /* Note that the Victor uses the following bounds for PWM values. These values
+   * were determined empirically and optimized for the Victor 888. These values
+   * should work reasonably well for Victor 884 controllers as well but if users
+   * experience issues such as asymmetric behavior around the deadband or
+   * inability to saturate the controller in either direction, calibration is
+   * recommended. The calibration procedure can be found in the Victor 884 User
+   * Manual available from IFI.
+   *
+   *   - 206 = full "forward"
+   *   - 131 = the "high end" of the deadband range
+   *   - 128 = center of the deadband range (off)
+   *   - 125 = the "low end" of the deadband range
+   *   - 56 = full "reverse"
+   */
+
+  SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+  SetPeriodMultiplier(kPeriodMultiplier_2X);
+  SetRaw(m_centerPwm);
+
+  LiveWindow::GetInstance()->AddActuator("Victor", GetChannel(), this);
+}
+
+/**
+ * Set the PWM value.
+ *
+ * The PWM value is set using a range of -1.0 to 1.0, appropriately
+ * scaling the value for the FPGA.
+ *
+ * @param speed The speed value between -1.0 and 1.0 to set.
+ */
+void Victor::Set(double speed) { SetSpeed(speed); }
+
+/**
+ * Get the recently set value of the PWM.
+ *
+ * @return The most recently set value for the PWM between -1.0 and 1.0.
+ */
+double Victor::Get() const { return GetSpeed(); }
+
+/**
+ * Common interface for disabling a motor.
+ */
+void Victor::Disable() { SetRaw(kPwmDisabled); }
+
+/**
+ * Write out the PID value as seen in the PIDOutput base object.
+ *
+ * @param output Write out the PWM value as was found in the PIDController
+ */
+void Victor::PIDWrite(double output) { Set(output); }
diff --git a/wpilibc/sim/src/XboxController.cpp b/wpilibc/sim/src/XboxController.cpp
new file mode 100644
index 0000000..441ecf0
--- /dev/null
+++ b/wpilibc/sim/src/XboxController.cpp
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. 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 "XboxController.h"
+
+#include "DriverStation.h"
+
+using namespace frc;
+
+/**
+ * Construct an instance of an Xbox controller.
+ *
+ * The joystick index is the USB port on the Driver Station.
+ *
+ * @param port The port on the Driver Station that the joystick is plugged into
+ *             (0-5).
+ */
+XboxController::XboxController(int port)
+    : GamepadBase(port), m_ds(DriverStation::GetInstance()) {}
+
+/**
+ * Get the X axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ */
+double XboxController::GetX(JoystickHand hand) const {
+  if (hand == kLeftHand) {
+    return GetRawAxis(0);
+  } else {
+    return GetRawAxis(4);
+  }
+}
+
+/**
+ * Get the Y axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ */
+double XboxController::GetY(JoystickHand hand) const {
+  if (hand == kLeftHand) {
+    return GetRawAxis(1);
+  } else {
+    return GetRawAxis(5);
+  }
+}
+
+/**
+ * Read the value of the bumper button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ */
+bool XboxController::GetBumper(JoystickHand hand) const {
+  if (hand == kLeftHand) {
+    return GetRawButton(5);
+  } else {
+    return GetRawButton(6);
+  }
+}
+
+/**
+ * Read the value of the stick button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetStickButton(JoystickHand hand) const {
+  if (hand == kLeftHand) {
+    return GetRawButton(9);
+  } else {
+    return GetRawButton(10);
+  }
+}
+
+/**
+ * Get the trigger axis value of the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ */
+double XboxController::GetTriggerAxis(JoystickHand hand) const {
+  if (hand == kLeftHand) {
+    return GetRawAxis(2);
+  } else {
+    return GetRawAxis(3);
+  }
+}
+
+/**
+ * Read the value of the A button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetAButton() const { return GetRawButton(1); }
+
+/**
+ * Read the value of the B button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetBButton() const { return GetRawButton(2); }
+
+/**
+ * Read the value of the X button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetXButton() const { return GetRawButton(3); }
+
+/**
+ * Read the value of the Y button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetYButton() const { return GetRawButton(4); }
+
+/**
+ * Read the value of the back button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetBackButton() const { return GetRawButton(7); }
+
+/**
+ * Read the value of the start button on the controller.
+ *
+ * @param hand Side of controller whose value should be returned.
+ * @return The state of the button.
+ */
+bool XboxController::GetStartButton() const { return GetRawButton(8); }
diff --git a/wpilibc/sim/src/simulation/SimContinuousOutput.cpp b/wpilibc/sim/src/simulation/SimContinuousOutput.cpp
new file mode 100644
index 0000000..9a907cf
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimContinuousOutput.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-2017. 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 "simulation/SimContinuousOutput.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimContinuousOutput::SimContinuousOutput(std::string topic) {
+  pub = MainNode::Advertise<gazebo::msgs::Float64>("~/simulator/" + topic);
+  std::cout << "Initialized ~/simulator/" + topic << std::endl;
+}
+
+void SimContinuousOutput::Set(double speed) {
+  gazebo::msgs::Float64 msg;
+  msg.set_data(speed);
+  pub->Publish(msg);
+}
+
+double SimContinuousOutput::Get() { return speed; }
diff --git a/wpilibc/sim/src/simulation/SimDigitalInput.cpp b/wpilibc/sim/src/simulation/SimDigitalInput.cpp
new file mode 100644
index 0000000..c4dbde6
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimDigitalInput.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-2017. 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 "simulation/SimDigitalInput.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimDigitalInput::SimDigitalInput(std::string topic) {
+  sub = MainNode::Subscribe("~/simulator/" + topic, &SimDigitalInput::callback,
+                            this);
+  std::cout << "Initialized ~/simulator/" + topic << std::endl;
+}
+
+bool SimDigitalInput::Get() { return value; }
+
+void SimDigitalInput::callback(const gazebo::msgs::ConstBoolPtr& msg) {
+  value = msg->data();
+}
diff --git a/wpilibc/sim/src/simulation/SimEncoder.cpp b/wpilibc/sim/src/simulation/SimEncoder.cpp
new file mode 100644
index 0000000..141a371
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimEncoder.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. 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 "simulation/SimEncoder.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimEncoder::SimEncoder(std::string topic) {
+  commandPub = MainNode::Advertise<gazebo::msgs::GzString>("~/simulator/" +
+                                                           topic + "/control");
+
+  posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
+                               &SimEncoder::positionCallback, this);
+  velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity",
+                               &SimEncoder::velocityCallback, this);
+
+  if (commandPub->WaitForConnection(
+          gazebo::common::Time(5.0))) {  // Wait up to five seconds.
+    std::cout << "Initialized ~/simulator/" + topic << std::endl;
+  } else {
+    std::cerr << "Failed to initialize ~/simulator/" + topic +
+                     ": does the encoder exist?"
+              << std::endl;
+  }
+}
+
+void SimEncoder::Reset() { sendCommand("reset"); }
+
+void SimEncoder::Start() { sendCommand("start"); }
+
+void SimEncoder::Stop() { sendCommand("stop"); }
+
+double SimEncoder::GetPosition() { return position; }
+
+double SimEncoder::GetVelocity() { return velocity; }
+
+void SimEncoder::sendCommand(std::string cmd) {
+  gazebo::msgs::GzString msg;
+  msg.set_data(cmd);
+  commandPub->Publish(msg);
+}
+
+void SimEncoder::positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  position = msg->data();
+}
+
+void SimEncoder::velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  velocity = msg->data();
+}
diff --git a/wpilibc/sim/src/simulation/SimFloatInput.cpp b/wpilibc/sim/src/simulation/SimFloatInput.cpp
new file mode 100644
index 0000000..8bf8ae3
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimFloatInput.cpp
@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014-2017. 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 "simulation/SimFloatInput.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimFloatInput::SimFloatInput(std::string topic) {
+  sub = MainNode::Subscribe("~/simulator/" + topic, &SimFloatInput::callback,
+                            this);
+  std::cout << "Initialized ~/simulator/" + topic << std::endl;
+}
+
+double SimFloatInput::Get() { return value; }
+
+void SimFloatInput::callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  value = msg->data();
+}
diff --git a/wpilibc/sim/src/simulation/SimGyro.cpp b/wpilibc/sim/src/simulation/SimGyro.cpp
new file mode 100644
index 0000000..33663bd
--- /dev/null
+++ b/wpilibc/sim/src/simulation/SimGyro.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. 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 "simulation/SimGyro.h"
+
+#include "simulation/MainNode.h"
+
+using namespace frc;
+
+SimGyro::SimGyro(std::string topic) {
+  commandPub = MainNode::Advertise<gazebo::msgs::GzString>("~/simulator/" +
+                                                           topic + "/control");
+
+  posSub = MainNode::Subscribe("~/simulator/" + topic + "/position",
+                               &SimGyro::positionCallback, this);
+  velSub = MainNode::Subscribe("~/simulator/" + topic + "/velocity",
+                               &SimGyro::velocityCallback, this);
+
+  if (commandPub->WaitForConnection(
+          gazebo::common::Time(5.0))) {  // Wait up to five seconds.
+    std::cout << "Initialized ~/simulator/" + topic << std::endl;
+  } else {
+    std::cerr << "Failed to initialize ~/simulator/" + topic +
+                     ": does the gyro exist?"
+              << std::endl;
+  }
+}
+
+void SimGyro::Reset() { sendCommand("reset"); }
+
+double SimGyro::GetAngle() { return position; }
+
+double SimGyro::GetVelocity() { return velocity; }
+
+void SimGyro::sendCommand(std::string cmd) {
+  gazebo::msgs::GzString msg;
+  msg.set_data(cmd);
+  commandPub->Publish(msg);
+}
+
+void SimGyro::positionCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  position = msg->data();
+}
+
+void SimGyro::velocityCallback(const gazebo::msgs::ConstFloat64Ptr& msg) {
+  velocity = msg->data();
+}