diff --git a/wpilibc/simulation/src/AnalogInput.cpp b/wpilibc/simulation/src/AnalogInput.cpp
new file mode 100644
index 0000000..71c48c5
--- /dev/null
+++ b/wpilibc/simulation/src/AnalogInput.cpp
@@ -0,0 +1,92 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "AnalogInput.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Construct an analog input.
+ * 
+ * @param channel The channel number to represent.
+ */
+AnalogInput::AnalogInput(uint32_t channel)
+{
+	m_channel = channel;
+	char buffer[50];
+	int n = sprintf(buffer, "analog/%d", channel);
+	m_impl = new SimFloatInput(buffer);
+
+	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.
+ */
+float 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.
+ */
+float AnalogInput::GetAverageVoltage() const
+{
+    return m_impl->Get();
+}
+
+/**
+ * Get the channel number.
+ * @return The channel number.
+ */
+uint32_t 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/simulation/src/AnalogPotentiometer.cpp b/wpilibc/simulation/src/AnalogPotentiometer.cpp
new file mode 100644
index 0000000..fa312c1
--- /dev/null
+++ b/wpilibc/simulation/src/AnalogPotentiometer.cpp
@@ -0,0 +1,77 @@
+
+#include "AnalogPotentiometer.h"
+
+/**
+ * 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/simulation/src/DigitalInput.cpp b/wpilibc/simulation/src/DigitalInput.cpp
new file mode 100644
index 0000000..e7ae36d
--- /dev/null
+++ b/wpilibc/simulation/src/DigitalInput.cpp
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "DigitalInput.h"
+#include "WPIErrors.h"
+
+/**
+ * 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(uint32_t channel)
+{
+	char buf[64];
+	m_channel = channel;
+	int n = sprintf(buf, "dio/%d", channel);
+	m_impl = new SimDigitalInput(buf);
+}
+
+/*
+ * Get the value from a digital input channel.
+ * Retrieve the value of a single digital input channel from the FPGA.
+ */
+uint32_t DigitalInput::Get() const
+{
+	return m_impl->Get();
+}
+
+/**
+ * @return The GPIO channel number that this object represents.
+ */
+uint32_t 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/simulation/src/DoubleSolenoid.cpp b/wpilibc/simulation/src/DoubleSolenoid.cpp
new file mode 100644
index 0000000..d1d0917
--- /dev/null
+++ b/wpilibc/simulation/src/DoubleSolenoid.cpp
@@ -0,0 +1,125 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "DoubleSolenoid.h"
+#include "WPIErrors.h"
+#include <string.h>
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * Constructor.
+ *
+ * @param forwardChannel The forward channel on the module to control.
+ * @param reverseChannel The reverse channel on the module to control.
+ */
+DoubleSolenoid::DoubleSolenoid(uint32_t forwardChannel, uint32_t 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(uint8_t moduleNumber, uint32_t forwardChannel, uint32_t reverseChannel)
+{
+    m_reversed = false;
+    if (reverseChannel < forwardChannel) { // Swap ports to get the right address
+        int channel = reverseChannel;
+        reverseChannel = forwardChannel;
+        forwardChannel = channel;
+        m_reversed = true;
+    }
+    char buffer[50];
+    int n = sprintf(buffer, "pneumatic/%d/%d/%d/%d", moduleNumber,
+                    forwardChannel, moduleNumber, reverseChannel);
+    m_impl = new SimContinuousOutput(buffer);  
+  
+	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/simulation/src/DriverStation.cpp b/wpilibc/simulation/src/DriverStation.cpp
new file mode 100644
index 0000000..39802ab
--- /dev/null
+++ b/wpilibc/simulation/src/DriverStation.cpp
@@ -0,0 +1,518 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "DriverStation.h"
+#include "AnalogInput.h"
+#include "Timer.h"
+#include "NetworkCommunication/FRCComm.h"
+#include "MotorSafetyHelper.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include <string.h>
+#include "Log.hpp"
+
+// set the logging level
+TLogLevel dsLogLevel = logDEBUG;
+const double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
+
+#define DS_LOG(level) \
+  if (level > dsLogLevel) ; \
+  else Log().Get(level)
+
+const uint32_t DriverStation::kJoystickPorts;
+
+/**
+ * DriverStation constructor.
+ *
+ * This is only called once the first time GetInstance() is called
+ */
+DriverStation::DriverStation() {
+  // All joysticks should default to having zero axes, povs and buttons, so
+  // uninitialized memory doesn't get sent to speed controllers.
+  for (unsigned int i = 0; i < kJoystickPorts; i++) {
+    m_joystickAxes[i].count = 0;
+    m_joystickPOVs[i].count = 0;
+    m_joystickButtons[i].count = 0;
+    m_joystickDescriptor[i].isXbox = 0;
+    m_joystickDescriptor[i].type = -1;
+    m_joystickDescriptor[i].name[0] = '\0';
+  }
+  // Register that semaphore with the network communications task.
+  // It will signal when new packet data is available.
+  HALSetNewDataSem(m_packetDataAvailableCond.native_handle());
+
+  AddToSingletonList();
+
+}
+
+void DriverStation::Run() {
+  int period = 0;
+  while (true) {
+    {
+      std::unique_lock<priority_mutex> lock(m_packetDataAvailableMutex);
+      m_packetDataAvailableCond.wait(lock);
+    }
+    GetData();
+    m_waitForDataCond.notify_all();
+
+    if (++period >= 4) {
+      MotorSafetyHelper::CheckMotors();
+      period = 0;
+    }
+    if (m_userInDisabled) HALNetworkCommunicationObserveUserProgramDisabled();
+    if (m_userInAutonomous) HALNetworkCommunicationObserveUserProgramAutonomous();
+    if (m_userInTeleop) HALNetworkCommunicationObserveUserProgramTeleop();
+    if (m_userInTest) HALNetworkCommunicationObserveUserProgramTest();
+  }
+}
+
+/**
+ * Return a reference to the singleton DriverStation.
+ * @return Pointer to the DS instance
+ */
+DriverStation &DriverStation::GetInstance() {
+  static DriverStation instance;
+  return instance;
+}
+
+/**
+ * Copy data from the DS task for the user.
+ * If no new data exists, it will just be returned, otherwise
+ * the data will be copied from the DS polling loop.
+ */
+void DriverStation::GetData() {
+  // Get the status of all of the joysticks
+  for (uint8_t stick = 0; stick < kJoystickPorts; stick++) {
+    HALGetJoystickAxes(stick, &m_joystickAxes[stick]);
+    HALGetJoystickPOVs(stick, &m_joystickPOVs[stick]);
+    HALGetJoystickButtons(stick, &m_joystickButtons[stick]);
+    HALGetJoystickDescriptor(stick, &m_joystickDescriptor[stick]);
+  }
+  m_newControlData.give();
+}
+
+/**
+ * Read the battery voltage.
+ *
+ * @return The battery voltage in Volts.
+ */
+float DriverStation::GetBatteryVoltage() const {
+  int32_t status = 0;
+  float voltage = getVinVoltage(&status);
+  wpi_setErrorWithContext(status, "getVinVoltage");
+
+  return voltage;
+}
+
+/**
+ * Reports errors related to unplugged joysticks
+ * Throttles the errors so that they don't overwhelm the DS
+ */
+void DriverStation::ReportJoystickUnpluggedError(std::string message) {
+  double currentTime = Timer::GetFPGATimestamp();
+  if (currentTime > m_nextMessageTime) {
+    ReportError(message);
+    m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+  }
+}
+
+/**
+ * Returns the number of axes on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of axes on the indicated joystick
+ */
+int DriverStation::GetStickAxisCount(uint32_t stick) const {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return 0;
+  }
+  HALJoystickAxes joystickAxes;
+  HALGetJoystickAxes(stick, &joystickAxes);
+  return joystickAxes.count;
+}
+
+/**
+ * Returns the name of the joystick at the given port
+ *
+ * @param stick The joystick port number
+ * @return The name of the joystick at the given port
+ */
+std::string DriverStation::GetJoystickName(uint32_t stick) const {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+  }
+  std::string retVal(m_joystickDescriptor[0].name);
+  return retVal;
+}
+
+/**
+ * Returns the type of joystick at a given port
+ *
+ * @param stick The joystick port number
+ * @return The HID type of joystick at the given port
+ */
+int DriverStation::GetJoystickType(uint32_t stick) const {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return -1;
+  }
+  return (int)m_joystickDescriptor[stick].type;
+}
+
+/**
+ * Returns a boolean indicating if the controller is an xbox controller.
+ *
+ * @param stick The joystick port number
+ * @return A boolean that is true if the controller is an xbox controller.
+ */
+bool DriverStation::GetJoystickIsXbox(uint32_t stick) const {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return false;
+  }
+  return (bool)m_joystickDescriptor[stick].isXbox;
+}
+
+/**
+ * Returns the types of Axes on a given joystick port
+ *
+ * @param stick The joystick port number and the target axis
+ * @return What type of axis the axis is reporting to be
+ */
+int DriverStation::GetJoystickAxisType(uint32_t stick, uint8_t axis) const {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return -1;
+  }
+  return m_joystickDescriptor[stick].axisTypes[axis];
+}
+
+/**
+ * Returns the number of POVs on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of POVs on the indicated joystick
+ */
+int DriverStation::GetStickPOVCount(uint32_t stick) const {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return 0;
+  }
+  HALJoystickPOVs joystickPOVs;
+  HALGetJoystickPOVs(stick, &joystickPOVs);
+  return joystickPOVs.count;
+}
+
+/**
+ * Returns the number of buttons on a given joystick port
+ *
+ * @param stick The joystick port number
+ * @return The number of buttons on the indicated joystick
+ */
+int DriverStation::GetStickButtonCount(uint32_t stick) const {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return 0;
+  }
+  HALJoystickButtons joystickButtons;
+  HALGetJoystickButtons(stick, &joystickButtons);
+  return joystickButtons.count;
+}
+
+/**
+ * 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.
+ */
+float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return 0;
+  }
+
+  if (axis >= m_joystickAxes[stick].count) {
+    if (axis >= kMaxJoystickAxes) {
+      wpi_setWPIError(BadJoystickAxis);
+    }
+    else {
+      ReportJoystickUnpluggedError(
+          "WARNING: Joystick Axis missing, check if all controllers are "
+          "plugged in\n");
+    }
+    return 0.0f;
+  }
+
+  int8_t value = m_joystickAxes[stick].axes[axis];
+
+  if (value < 0) {
+    return value / 128.0f;
+  } else {
+    return value / 127.0f;
+  }
+}
+
+/**
+ * Get the state of a POV on the joystick.
+ *
+ * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ */
+int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return -1;
+  }
+
+  if (pov >= m_joystickPOVs[stick].count) {
+    if (pov >= kMaxJoystickPOVs) {
+      wpi_setWPIError(BadJoystickAxis);
+    }
+    else {
+      ReportJoystickUnpluggedError(
+          "WARNING: Joystick POV missing, check if all controllers are plugged "
+          "in\n");
+    }
+    return -1;
+  }
+
+  return m_joystickPOVs[stick].povs[pov];
+}
+
+/**
+ * The state of the buttons on the joystick.
+ *
+ * @param stick The joystick to read.
+ * @return The state of the buttons on the joystick.
+ */
+uint32_t DriverStation::GetStickButtons(uint32_t stick) const {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return 0;
+  }
+
+  return m_joystickButtons[stick].buttons;
+}
+
+/**
+ * The state of one joystick button. Button indexes begin at 1.
+ *
+ * @param stick The joystick to read.
+ * @param button The button index, beginning at 1.
+ * @return The state of the joystick button.
+ */
+bool DriverStation::GetStickButton(uint32_t stick, uint8_t button) {
+  if (stick >= kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+    return false;
+  }
+
+  if (button > m_joystickButtons[stick].count) {
+    ReportJoystickUnpluggedError(
+        "WARNING: Joystick Button missing, check if all controllers are "
+        "plugged in\n");
+    return false;
+  }
+  if (button == 0) {
+    ReportJoystickUnpluggedError(
+        "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+    return false;
+  }
+  return ((0x1 << (button - 1)) & m_joystickButtons[stick].buttons) != 0;
+}
+
+/**
+ * Check if the DS has enabled the robot
+ * @return True if the robot is enabled and the DS is connected
+ */
+bool DriverStation::IsEnabled() const {
+  HALControlWord controlWord;
+  memset(&controlWord, 0, sizeof(controlWord));
+  HALGetControlWord(&controlWord);
+  return controlWord.enabled && controlWord.dsAttached;
+}
+
+/**
+ * Check if the robot is disabled
+ * @return True if the robot is explicitly disabled or the DS is not connected
+ */
+bool DriverStation::IsDisabled() const {
+  HALControlWord controlWord;
+  memset(&controlWord, 0, sizeof(controlWord));
+  HALGetControlWord(&controlWord);
+  return !(controlWord.enabled && controlWord.dsAttached);
+}
+
+/**
+ * Check if the DS is commanding autonomous mode
+ * @return True if the robot is being commanded to be in autonomous mode
+ */
+bool DriverStation::IsAutonomous() const {
+  HALControlWord controlWord;
+  memset(&controlWord, 0, sizeof(controlWord));
+  HALGetControlWord(&controlWord);
+  return controlWord.autonomous;
+}
+
+/**
+ * Check if the DS is commanding teleop mode
+ * @return True if the robot is being commanded to be in teleop mode
+ */
+bool DriverStation::IsOperatorControl() const {
+  HALControlWord controlWord;
+  memset(&controlWord, 0, sizeof(controlWord));
+  HALGetControlWord(&controlWord);
+  return !(controlWord.autonomous || controlWord.test);
+}
+
+/**
+ * Check if the DS is commanding test mode
+ * @return True if the robot is being commanded to be in test mode
+ */
+bool DriverStation::IsTest() const {
+  HALControlWord controlWord;
+  HALGetControlWord(&controlWord);
+  return controlWord.test;
+}
+
+/**
+ * Check if the DS is attached
+ * @return True if the DS is connected to the robot
+ */
+bool DriverStation::IsDSAttached() const {
+  HALControlWord controlWord;
+  memset(&controlWord, 0, sizeof(controlWord));
+  HALGetControlWord(&controlWord);
+  return controlWord.dsAttached;
+}
+
+/**
+ * @return always true in simulation
+ */
+bool DriverStation::IsSysActive() const {
+  return true;
+}
+
+/**
+ * @return always false in simulation
+ */
+bool DriverStation::IsSysBrownedOut() const {
+  return false;
+}
+
+/**
+ * Has a new control packet from the driver station arrived since the last time
+ * this function was called?
+ * Warning: If you call this function from more than one place at the same time,
+ * you will not get the get the intended behaviour.
+ * @return True if the control data has been updated since the last call.
+ */
+bool DriverStation::IsNewControlData() const {
+  return m_newControlData.tryTake() == 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 {
+  HALControlWord controlWord;
+  HALGetControlWord(&controlWord);
+  return controlWord.fmsAttached;
+}
+
+/**
+ * Return the alliance that the driver station says it is on.
+ * This could return kRed or kBlue
+ * @return The Alliance enum (kRed, kBlue or kInvalid)
+ */
+DriverStation::Alliance DriverStation::GetAlliance() const {
+  HALAllianceStationID allianceStationID;
+  HALGetAllianceStation(&allianceStationID);
+  switch (allianceStationID) {
+    case kHALAllianceStationID_red1:
+    case kHALAllianceStationID_red2:
+    case kHALAllianceStationID_red3:
+      return kRed;
+    case kHALAllianceStationID_blue1:
+    case kHALAllianceStationID_blue2:
+    case kHALAllianceStationID_blue3:
+      return kBlue;
+    default:
+      return kInvalid;
+  }
+}
+
+/**
+ * Return the driver station location on the field
+ * This could return 1, 2, or 3
+ * @return The location of the driver station (1-3, 0 for invalid)
+ */
+uint32_t DriverStation::GetLocation() const {
+  HALAllianceStationID allianceStationID;
+  HALGetAllianceStation(&allianceStationID);
+  switch (allianceStationID) {
+    case kHALAllianceStationID_red1:
+    case kHALAllianceStationID_blue1:
+      return 1;
+    case kHALAllianceStationID_red2:
+    case kHALAllianceStationID_blue2:
+      return 2;
+    case kHALAllianceStationID_red3:
+    case kHALAllianceStationID_blue3:
+      return 3;
+    default:
+      return 0;
+  }
+}
+
+/**
+ * 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() {
+  std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
+  m_waitForDataCond.wait(lock);
+}
+
+/**
+ * Return the approximate match time
+ * The FMS does not send an official match time to the robots, but does send an
+ * approximate match time.
+ * The value will count down the time remaining in the current period (auto or
+ * teleop).
+ * Warning: This is not an official time (so it cannot be used to dispute ref
+ * calls or guarantee that a function
+ * will trigger before the match ends)
+ * The Practice Match function of the DS approximates the behaviour seen on the
+ * field.
+ * @return Time remaining in current match period (auto or teleop)
+ */
+double DriverStation::GetMatchTime() const {
+  float matchTime;
+  HALGetMatchTime(&matchTime);
+  return (double)matchTime;
+}
+
+/**
+ * Report an error to the DriverStation messages window.
+ * The error is also printed to the program console.
+ */
+void DriverStation::ReportError(std::string error) {
+  std::cout << error << std::endl;
+
+  HALControlWord controlWord;
+  HALGetControlWord(&controlWord);
+  if (controlWord.dsAttached) {
+    HALSetErrorData(error.c_str(), error.size(), 0);
+  }
+}
diff --git a/wpilibc/simulation/src/Encoder.cpp b/wpilibc/simulation/src/Encoder.cpp
new file mode 100644
index 0000000..790868e
--- /dev/null
+++ b/wpilibc/simulation/src/Encoder.cpp
@@ -0,0 +1,364 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Encoder.h"
+#include "Resource.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * 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;
+
+	int32_t 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;
+	}
+	char buffer[50];
+	int n = sprintf(buffer, "dio/%d/%d", channelA, channelB);
+	impl = new SimEncoder(buffer);
+	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 represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @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(uint32_t aChannel, uint32_t 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 represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @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 represents the orientation of the encoder and inverts the output values
+ * if necessary so forward represents positive values.
+ * @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.
+ */
+int32_t 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
+ */
+int32_t 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.
+ */
+int32_t 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/simulation/src/Gyro.cpp b/wpilibc/simulation/src/Gyro.cpp
new file mode 100644
index 0000000..a28bb9b
--- /dev/null
+++ b/wpilibc/simulation/src/Gyro.cpp
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Gyro.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+const uint32_t Gyro::kOversampleBits = 10;
+const uint32_t Gyro::kAverageBits = 0;
+const float Gyro::kSamplesPerSecond = 50.0;
+const float Gyro::kCalibrationSampleTime = 5.0;
+const float Gyro::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 Gyro::InitGyro(int channel)
+{
+	SetPIDSourceType(PIDSourceType::kDisplacement);
+
+	char buffer[50];
+	int n = sprintf(buffer, "analog/%d", channel);
+	impl = new SimGyro(buffer);
+
+	LiveWindow::GetInstance()->AddSensor("Gyro", channel, this);
+}
+
+/**
+ * Gyro constructor with only a channel..
+ *
+ * @param channel The analog channel the gyro is connected to.
+ */
+Gyro::Gyro(uint32_t channel)
+{
+    InitGyro(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 Gyro::Reset()
+{
+    impl->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.
+ */
+float Gyro::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 Gyro::GetRate() const
+{
+    return impl->GetVelocity();
+}
+
+void Gyro::SetPIDSourceType(PIDSourceType pidSource)
+{
+    m_pidSource = pidSource;
+}
+
+/**
+ * Get the angle in degrees for the PIDSource base object.
+ *
+ * @return The angle in degrees.
+ */
+double Gyro::PIDGet()
+{
+	switch(GetPIDSourceType()){
+	case PIDSourceType::kRate:
+		return GetRate();
+	case PIDSourceType::kDisplacement:
+		return GetAngle();
+	default:
+		return 0;
+	}
+}
+
+void Gyro::UpdateTable() {
+	if (m_table != nullptr) {
+		m_table->PutNumber("Value", GetAngle());
+	}
+}
+
+void Gyro::StartLiveWindowMode() {
+
+}
+
+void Gyro::StopLiveWindowMode() {
+
+}
+
+std::string Gyro::GetSmartDashboardType() const {
+	return "Gyro";
+}
+
+void Gyro::InitTable(std::shared_ptr<ITable> subTable) {
+	m_table = subTable;
+	UpdateTable();
+}
+
+std::shared_ptr<ITable> Gyro::GetTable() const {
+	return m_table;
+}
diff --git a/wpilibc/simulation/src/IterativeRobot.cpp b/wpilibc/simulation/src/IterativeRobot.cpp
new file mode 100644
index 0000000..f2511df
--- /dev/null
+++ b/wpilibc/simulation/src/IterativeRobot.cpp
@@ -0,0 +1,312 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+#include "IterativeRobot.h"
+
+#include "DriverStation.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+//not sure what this is used for yet.
+#ifdef _UNIX
+	#include <unistd.h>
+#endif
+
+const double IterativeRobot::kDefaultPeriod = 0;
+
+/**
+ * Set the period for the periodic functions.
+ *
+ * @param period The period of the periodic function calls.  0.0 means sync to driver station control data.
+ */
+void IterativeRobot::SetPeriod(double period)
+{
+	if (period > 0.0)
+	{
+		// Not syncing with the DS, so start the timer for the main loop
+		m_mainLoopTimer.Reset();
+		m_mainLoopTimer.Start();
+	}
+	else
+	{
+		// Syncing with the DS, don't need the timer
+		m_mainLoopTimer.Stop();
+	}
+	m_period = period;
+}
+
+/**
+ * Get the period for the periodic functions.
+ * Returns 0.0 if configured to syncronize with DS control data packets.
+ * @return Period of the periodic function calls
+ */
+double IterativeRobot::GetPeriod()
+{
+	return m_period;
+}
+
+/**
+ * Get the number of loops per second for the IterativeRobot
+ * @return Frequency of the periodic function calls
+ */
+double IterativeRobot::GetLoopsPerSec()
+{
+	// If syncing to the driver station, we don't know the rate,
+	//   so guess something close.
+	if (m_period <= 0.0)
+		return 50.0;
+	return 1.0 / m_period;
+}
+
+/**
+ * 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
+	SmartDashboard::init();
+	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;
+			}
+			if (NextPeriodReady())
+			{
+				// 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;
+			}
+			if (NextPeriodReady())
+			{
+				// 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;
+            }
+            if (NextPeriodReady())
+            {
+                // 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);
+			}
+			if (NextPeriodReady())
+			{
+				// TODO: HALNetworkCommunicationObserveUserProgramTeleop();
+				TeleopPeriodic();
+			}
+		}
+		// wait for driver station data so the loop doesn't hog the CPU
+		m_ds.WaitForData();
+	}
+}
+
+/**
+ * Determine if the periodic functions should be called.
+ *
+ * If m_period > 0.0, call the periodic function every m_period as compared
+ * to Timer.Get().  If m_period == 0.0, call the periodic functions whenever
+ * a packet is received from the Driver Station, or about every 20ms.
+ *
+ * @todo Decide what this should do if it slips more than one cycle.
+ */
+
+bool IterativeRobot::NextPeriodReady()
+{
+	if (m_period > 0.0)
+	{
+		return m_mainLoopTimer.HasPeriodPassed(m_period);
+	}
+	else
+	{
+		// XXX: BROKEN! return m_ds->IsNewControlData();
+	}
+    return true;
+}
+
+/**
+ * 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()
+{
+	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()
+{
+	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()
+{
+	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()
+{
+    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()
+{
+    printf("Default %s() method... Overload me!\n", __FUNCTION__);
+}
+
+/**
+ * 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)
+	{
+		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)
+	{
+		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)
+	{
+		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)
+    {
+        printf("Default %s() method... Overload me!\n", __FUNCTION__);
+        firstRun = false;
+    }
+}
+
diff --git a/wpilibc/simulation/src/Jaguar.cpp b/wpilibc/simulation/src/Jaguar.cpp
new file mode 100644
index 0000000..fab109d
--- /dev/null
+++ b/wpilibc/simulation/src/Jaguar.cpp
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+
+#include "Jaguar.h"
+//#include "NetworkCommunication/UsageReporting.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * @param channel The PWM channel that the Jaguar is attached to.
+ */
+Jaguar::Jaguar(uint32_t 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.
+ * @param syncGroup Unused interface.
+ */
+void Jaguar::Set(float speed, uint8_t syncGroup)
+{
+	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.
+ */
+float 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(float output)
+{
+	Set(output);
+}
diff --git a/wpilibc/simulation/src/Joystick.cpp b/wpilibc/simulation/src/Joystick.cpp
new file mode 100644
index 0000000..a7a299c
--- /dev/null
+++ b/wpilibc/simulation/src/Joystick.cpp
@@ -0,0 +1,377 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Joystick.h"
+#include "DriverStation.h"
+//#include "NetworkCommunication/UsageReporting.h"
+#include "WPIErrors.h"
+#include <math.h>
+#include <string.h>
+
+const uint32_t Joystick::kDefaultXAxis;
+const uint32_t Joystick::kDefaultYAxis;
+const uint32_t Joystick::kDefaultZAxis;
+const uint32_t Joystick::kDefaultTwistAxis;
+const uint32_t Joystick::kDefaultThrottleAxis;
+const uint32_t Joystick::kDefaultTriggerButton;
+const uint32_t 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 drivers station.
+ *
+ * @param port The port on the driver station that the joystick is plugged into
+ * (0-5).
+ */
+Joystick::Joystick(uint32_t 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;
+
+  HALReport(HALUsageReporting::kResourceType_Joystick, port);
+}
+
+/**
+ * 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(uint32_t port, uint32_t numAxisTypes,
+                   uint32_t numButtonTypes)
+    : m_ds(DriverStation::GetInstance()),
+      m_port(port),
+      m_axes(numAxisTypes),
+      m_buttons(numButtonTypes) {
+  if (!joySticksInitialized) {
+    for (auto& joystick : joysticks) joystick = nullptr;
+    joySticksInitialized = true;
+  }
+  if (m_port >= DriverStation::kJoystickPorts) {
+    wpi_setWPIError(BadJoystickIndex);
+  } else {
+    joysticks[m_port] = this;
+  }
+}
+
+Joystick *Joystick::GetStickForPort(uint32_t 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.
+ */
+float 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.
+ */
+float 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.
+ */
+float Joystick::GetZ() 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.
+ */
+float 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.
+ */
+float Joystick::GetThrottle() const {
+  return GetRawAxis(m_axes[kThrottleAxis]);
+}
+
+/**
+ * Get the value of the axis.
+ *
+ * @param axis The axis to read, starting at 0.
+ * @return The value of the axis.
+ */
+float Joystick::GetRawAxis(uint32_t axis) const {
+  return m_ds.GetStickAxis(m_port, axis);
+}
+
+/**
+ * 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.
+ */
+float 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]);
+}
+
+/**
+ * This is not supported for the Joystick.
+ * This method is only here to complete the GenericHID interface.
+ */
+bool Joystick::GetBumper(JoystickHand hand) const {
+  // Joysticks don't have bumpers.
+  return false;
+}
+
+/**
+ * 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 Joystick::GetRawButton(uint32_t button) const {
+  return m_ds.GetStickButton(m_port, button);
+}
+
+/**
+ * Get the state of a POV on the joystick.
+ *
+ * @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 Joystick::GetPOV(uint32_t pov) const {
+  return m_ds.GetStickPOV(m_port, pov);
+}
+
+/**
+ * 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 m_ds.GetStickAxisCount(m_port); }
+
+/**
+ * Get the value of isXbox for the joystick.
+ *
+ * @return A boolean that is true if the joystick is an xbox controller.
+ */
+bool Joystick::GetIsXbox() const { return m_ds.GetJoystickIsXbox(m_port); }
+
+/**
+ * Get the HID type of the controller.
+ *
+ * @return the HID type of the controller.
+ */
+Joystick::HIDType Joystick::GetType() const {
+  return static_cast<HIDType>(m_ds.GetJoystickType(m_port));
+}
+
+/**
+ * Get the name of the joystick.
+ *
+ * @return the name of the controller.
+ */
+std::string Joystick::GetName() const { return m_ds.GetJoystickName(m_port); }
+
+// int Joystick::GetAxisType(uint8_t axis) const
+//{
+//	return m_ds.GetJoystickAxisType(m_port, axis);
+//}
+
+/**
+  * Get the number of axis for a joystick
+  *
+* @return the number of buttons on the current joystick
+ */
+int Joystick::GetButtonCount() const {
+  return m_ds.GetStickButtonCount(m_port);
+}
+
+/**
+ * Get the number of axis for a joystick
+ *
+ * @return then umber of POVs for the current joystick
+ */
+int Joystick::GetPOVCount() const { return m_ds.GetStickPOVCount(m_port); }
+
+/**
+ * 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.
+ */
+uint32_t 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, uint32_t 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
+ */
+float Joystick::GetMagnitude() const {
+  return sqrt(pow(GetX(), 2) + 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
+ */
+float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); }
+
+/**
+ * Get the direction of the vector formed by the joystick and its origin
+ * in degrees
+ *
+ * uses acos(-1) to represent Pi due to absence of readily accessible Pi
+ * constant in C++
+ *
+ * @return The direction of the vector in degrees
+ */
+float Joystick::GetDirectionDegrees() const {
+  return (180 / acos(-1)) * GetDirectionRadians();
+}
+
+/**
+ * Set the rumble output for the joystick. 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 Joystick::SetRumble(RumbleType type, float value) {
+  if (value < 0)
+    value = 0;
+  else if (value > 1)
+    value = 1;
+  if (type == kLeftRumble)
+    m_leftRumble = value * 65535;
+  else
+    m_rightRumble = value * 65535;
+  HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+/**
+ * Set a single HID output value for the joystick.
+ * @param outputNumber The index of the output to set (1-32)
+ * @param value The value to set the output to
+ */
+
+void Joystick::SetOutput(uint8_t outputNumber, bool value) {
+  m_outputs =
+      (m_outputs & ~(1 << (outputNumber - 1))) | (value << (outputNumber - 1));
+
+  HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
+
+/**
+ * Set all HID output values for the joystick.
+ * @param value The 32 bit output value (1 bit for each output)
+ */
+void Joystick::SetOutputs(uint32_t value) {
+  m_outputs = value;
+  HALSetJoystickOutputs(m_port, m_outputs, m_leftRumble, m_rightRumble);
+}
diff --git a/wpilibc/simulation/src/MotorSafetyHelper.cpp b/wpilibc/simulation/src/MotorSafetyHelper.cpp
new file mode 100644
index 0000000..87f5352
--- /dev/null
+++ b/wpilibc/simulation/src/MotorSafetyHelper.cpp
@@ -0,0 +1,140 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.
+ */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "MotorSafetyHelper.h"
+
+#include "DriverStation.h"
+#include "MotorSafety.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+#include <stdio.h>
+#include <sstream>
+
+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(float 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.
+ */
+float 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/simulation/src/Notifier.cpp b/wpilibc/simulation/src/Notifier.cpp
new file mode 100644
index 0000000..25daaf0
--- /dev/null
+++ b/wpilibc/simulation/src/Notifier.cpp
@@ -0,0 +1,261 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Notifier.h"
+#include "Timer.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+Notifier *Notifier::timerQueueHead = nullptr;
+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, void *param)
+{
+	if (handler == nullptr)
+		wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+	m_handler = handler;
+	m_param = param;
+	m_periodic = false;
+	m_expirationTime = 0;
+	m_period = 0;
+	m_nextEvent = nullptr;
+	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(uint32_t mask, void *params)
+{
+	Notifier *current;
+	while (true)				// keep processing past events until no more
+	{
+		{
+			std::lock_guard<priority_recursive_mutex> sync(queueMutex);
+			double currentTime = GetClock();
+			current = timerQueueHead;
+			if (current == nullptr || current->m_expirationTime > currentTime)
+			{
+				break;		// no more timer events to process
+			}
+			// need to process this entry
+			timerQueueHead = current->m_nextEvent;
+			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(current->m_param);	// 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;
+	}
+	if (timerQueueHead == nullptr || timerQueueHead->m_expirationTime >= this->m_expirationTime)
+	{
+		// the queue is empty or greater than the new entry
+		// the new entry becomes the first element
+		this->m_nextEvent = timerQueueHead;
+		timerQueueHead = this;
+		if (!reschedule)
+		{
+			// since the first element changed, update alarm, unless we already plan to
+			UpdateAlarm();
+		}
+	}
+	else
+	{
+		for (Notifier **npp = &(timerQueueHead->m_nextEvent); ; npp = &(*npp)->m_nextEvent)
+		{
+			Notifier *n = *npp;
+			if (n == nullptr || n->m_expirationTime > this->m_expirationTime)
+			{
+				*npp = this;
+				this->m_nextEvent = n;
+				break;
+			}
+		}
+	}
+	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(timerQueueHead != nullptr);
+		if (timerQueueHead == this)
+		{
+			// remove the first item in the list - update the alarm
+			timerQueueHead = this->m_nextEvent;
+			UpdateAlarm();
+		}
+		else
+		{
+			for (Notifier *n = timerQueueHead; n != nullptr; n = n->m_nextEvent)
+			{
+				if (n->m_nextEvent == this)
+				{
+					// this element is the next element from *n from the queue
+					n->m_nextEvent = this->m_nextEvent;	// point around this one
+				}
+			}
+		}
+	}
+}
+
+/**
+ * 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);
+        if (timerQueueHead != nullptr)
+        {
+            Wait(timerQueueHead->m_expirationTime - GetClock());
+        }
+        else
+        {
+            Wait(0.05);
+        }
+    }
+}
diff --git a/wpilibc/simulation/src/PIDController.cpp b/wpilibc/simulation/src/PIDController.cpp
new file mode 100644
index 0000000..03beb5c
--- /dev/null
+++ b/wpilibc/simulation/src/PIDController.cpp
@@ -0,0 +1,619 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "PIDController.h"
+#include "Notifier.h"
+#include "PIDSource.h"
+#include "PIDOutput.h"
+#include <math.h>
+
+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(float Kp, float Ki, float Kd,
+								PIDSource *source, PIDOutput *output,
+								float period)
+{
+	Initialize(Kp, Ki, Kd, 0.0f, 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(float Kp, float Ki, float Kd, float Kf,
+								PIDSource *source, PIDOutput *output,
+								float period)
+{
+	Initialize(Kp, Ki, Kd, Kf, source, output, period);
+}
+
+
+void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
+								PIDSource *source, PIDOutput *output,
+								float 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_prevInput = 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::CallCalculate, this);
+	m_controlLoop->StartPeriodic(m_period);
+
+	static int32_t instances = 0;
+	instances++;
+
+	m_toleranceType = kNoTolerance;
+}
+
+PIDController::~PIDController() {
+	if (m_table != nullptr) m_table->RemoveTableListener(this);
+}
+
+/**
+ * Call the Calculate method as a non-static method. This avoids having to prepend
+ * all local variables in that method with the class pointer. This way the "this"
+ * pointer will be set up and class variables can be called more easily.
+ * This method is static and called by the Notifier class.
+ * @param controller the address of the PID controller object to use in the background loop
+ */
+void PIDController::CallCalculate(void *controller)
+{
+	PIDController *control = (PIDController*) controller;
+	control->Calculate();
+}
+
+ /**
+  * Read the input, calculate the output accordingly, and write to the output.
+  * This should only be called by the Notifier indirectly through CallCalculate
+  * and is created during initialization.
+  */
+void PIDController::Calculate()
+{
+	bool enabled;
+	PIDSource *pidInput;
+
+	{
+		std::lock_guard<priority_mutex> lock(m_mutex);
+		if (m_pidInput == 0) return;
+		if (m_pidOutput == 0) return;
+		enabled = m_enabled;
+		pidInput = m_pidInput;
+	}
+
+	if (enabled)
+	{
+		float input = pidInput->PIDGet();
+		float result;
+		PIDOutput *pidOutput;
+
+		{
+			std::lock_guard<priority_mutex> sync(m_mutex);
+			m_error = m_setpoint - input;
+			if (m_continuous)
+			{
+				if (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 + m_setpoint * m_F;
+            }
+            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_prevInput - input) + m_setpoint * m_F;
+            }
+			m_prevInput = input;
+
+			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);
+	}
+}
+
+/**
+ * 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_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_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_mutex> lock(m_mutex);
+	return m_P;
+}
+
+/**
+ * Get the Integral coefficient
+ * @return integral coefficient
+ */
+double PIDController::GetI() const
+{
+	std::lock_guard<priority_mutex> lock(m_mutex);
+	return m_I;
+}
+
+/**
+ * Get the Differential coefficient
+ * @return differential coefficient
+ */
+double PIDController::GetD() const
+{
+	std::lock_guard<priority_mutex> lock(m_mutex);
+	return m_D;
+}
+
+/**
+ * Get the Feed forward coefficient
+ * @return Feed forward coefficient
+ */
+double PIDController::GetF() const
+{
+	std::lock_guard<priority_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
+ */
+float PIDController::Get() const
+{
+	std::lock_guard<priority_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_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(float minimumInput, float maximumInput)
+{
+	{
+		std::lock_guard<priority_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(float minimumOutput, float maximumOutput)
+{
+	std::lock_guard<priority_mutex> lock(m_mutex);
+	m_minimumOutput = minimumOutput;
+	m_maximumOutput = maximumOutput;
+}
+
+/**
+ * Set the setpoint for the PIDController
+ * @param setpoint the desired setpoint
+ */
+void PIDController::SetSetpoint(float setpoint)
+{
+	{
+		std::lock_guard<priority_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_mutex> lock(m_mutex);
+	return m_setpoint;
+}
+
+/**
+ * Retruns the current difference of the input from the setpoint
+ * @return the current error
+ */
+float PIDController::GetError() const
+{
+	double pidInput;
+	{
+		std::lock_guard<priority_mutex> lock(m_mutex);
+		pidInput = m_pidInput->PIDGet();
+	}
+	return GetSetpoint() - pidInput;
+}
+
+/**
+ * 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
+ */
+float PIDController::GetAvgError() const {
+  float avgError = 0;
+  {
+    std::lock_guard<priority_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 percentage error which is tolerable
+ */
+void PIDController::SetTolerance(float percent)
+{
+	std::lock_guard<priority_mutex> lock(m_mutex);
+	m_toleranceType = kPercentTolerance;
+	m_tolerance = percent;
+}
+
+/*
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDController::SetPercentTolerance(float percent)
+{
+	std::lock_guard<priority_mutex> lock(m_mutex);
+	m_toleranceType = kPercentTolerance;
+	m_tolerance = percent;
+}
+
+/*
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDController::SetAbsoluteTolerance(float absTolerance)
+{
+	std::lock_guard<priority_mutex> lock(m_mutex);
+	m_toleranceType = kAbsoluteTolerance;
+	m_tolerance = absTolerance;
+}
+
+/*
+ * 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(unsigned bufLength) {
+  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
+{
+	double error = GetError();
+
+	std::lock_guard<priority_mutex> sync(m_mutex);
+	switch (m_toleranceType) {
+	case kPercentTolerance:
+		return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
+		break;
+	case kAbsoluteTolerance:
+		return 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_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_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_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_mutex> lock(m_mutex);
+	m_prevInput = 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);
+	}
+}
+
+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/simulation/src/PWM.cpp b/wpilibc/simulation/src/PWM.cpp
new file mode 100644
index 0000000..dfb8bd9
--- /dev/null
+++ b/wpilibc/simulation/src/PWM.cpp
@@ -0,0 +1,261 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "PWM.h"
+
+//#include "NetworkCommunication/UsageReporting.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+
+const float PWM::kDefaultPwmPeriod = 5.05;
+const float PWM::kDefaultPwmCenter = 1.5;
+const int32_t PWM::kDefaultPwmStepsDown = 1000;
+const int32_t 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(uint32_t channel)
+{
+	char buf[64];
+
+	if (!CheckPWMChannel(channel))
+	{
+		snprintf(buf, 64, "PWM Channel %d", channel);
+		wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
+		return;
+	}
+
+	sprintf(buf, "pwm/%d", channel);
+	impl = new SimContinuousOutput(buf);
+	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(int32_t max, int32_t deadbandMax, int32_t center, int32_t deadbandMin, int32_t 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(float 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.
+ */
+float PWM::GetPosition() const
+{
+	float 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(float 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.
+ */
+float PWM::GetSpeed() const
+{
+	float 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(unsigned short 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/simulation/src/Relay.cpp b/wpilibc/simulation/src/Relay.cpp
new file mode 100644
index 0000000..f35d782
--- /dev/null
+++ b/wpilibc/simulation/src/Relay.cpp
@@ -0,0 +1,249 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Relay.h"
+
+#include "MotorSafetyHelper.h"
+//#include "NetworkCommunication/UsageReporting.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * 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(uint32_t channel, Relay::Direction direction)
+	: m_channel (channel)
+	, m_direction (direction)
+{
+	char buf[64];
+	if (!SensorBase::CheckRelayChannel(m_channel))
+	{
+		snprintf(buf, 64, "Relay Channel %d", m_channel);
+		wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
+		return;
+	}
+
+    m_safetyHelper = std::make_unique<MotorSafetyHelper>(this);
+    m_safetyHelper->SetSafetyEnabled(false);
+
+	sprintf(buf, "relay/%d", m_channel);
+	impl = new SimContinuousOutput(buf); // 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;
+	}
+}
+
+uint32_t 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(float timeout) {
+  m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the relay object.
+ * @return The expiration time value.
+ */
+float Relay::GetExpiration() const { return m_safetyHelper->GetExpiration(); }
+
+/**
+ * Check if the relay object is currently alive or stopped due to a timeout.
+ * @returns 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
+ * @returns 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/simulation/src/RobotBase.cpp b/wpilibc/simulation/src/RobotBase.cpp
new file mode 100644
index 0000000..1fa5ac7
--- /dev/null
+++ b/wpilibc/simulation/src/RobotBase.cpp
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotBase.h"
+#include "RobotState.h"
+#include "Utility.h"
+
+#include <cstring>
+
+RobotBase* RobotBase::m_instance = nullptr;
+
+void RobotBase::setInstance(RobotBase* robot)
+{
+	wpi_assert(m_instance == nullptr);
+	m_instance = robot;
+}
+
+RobotBase &RobotBase::getInstance()
+{
+	return *m_instance;
+}
+
+/**
+ * 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());
+	transport::SubscriberPtr time_pub = 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();
+}
+
+/**
+ * This class exists for the sole purpose of getting its destructor called when the module unloads.
+ * Before the module is done unloading, we need to delete the RobotBase derived singleton.  This should delete
+ * the other remaining singletons that were registered.  This should also stop all tasks that are using
+ * the Task class.
+ */
+class RobotDeleter
+{
+public:
+	~RobotDeleter()
+	{
+		delete &RobotBase::getInstance();
+	}
+};
+static RobotDeleter g_robotDeleter;
diff --git a/wpilibc/simulation/src/RobotDrive.cpp b/wpilibc/simulation/src/RobotDrive.cpp
new file mode 100644
index 0000000..852d0bf
--- /dev/null
+++ b/wpilibc/simulation/src/RobotDrive.cpp
@@ -0,0 +1,734 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "RobotDrive.h"
+
+//#include "CANJaguar.h"
+#include "GenericHID.h"
+#include "Joystick.h"
+#include "Jaguar.h"
+#include "Utility.h"
+#include "WPIErrors.h"
+#include <math.h>
+
+#undef max
+#include <algorithm>
+
+const int32_t RobotDrive::kMaxNumberOfMotors;
+
+/*
+ * 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 Jaguars 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(uint32_t leftMotorChannel, uint32_t rightMotorChannel)
+{
+	InitRobotDrive();
+	m_rearLeftMotor = new Jaguar(leftMotorChannel);
+	m_rearRightMotor = new Jaguar(rightMotorChannel);
+	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+	{
+		m_invertedMotors[i] = 1;
+	}
+	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 Jaguars 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(uint32_t frontLeftMotor, uint32_t rearLeftMotor,
+		uint32_t frontRightMotor, uint32_t rearRightMotor)
+{
+	InitRobotDrive();
+	m_rearLeftMotor = new Jaguar(rearLeftMotor);
+	m_rearRightMotor = new Jaguar(rearRightMotor);
+	m_frontLeftMotor = new Jaguar(frontLeftMotor);
+	m_frontRightMotor = new Jaguar(frontRightMotor);
+	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+	{
+		m_invertedMotors[i] = 1;
+	}
+	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 = leftMotor;
+	m_rearRightMotor = rightMotor;
+	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+	{
+		m_invertedMotors[i] = 1;
+	}
+	m_deleteSpeedControllers = false;
+}
+
+RobotDrive::RobotDrive(SpeedController &leftMotor, SpeedController &rightMotor)
+{
+	InitRobotDrive();
+	m_rearLeftMotor = &leftMotor;
+	m_rearRightMotor = &rightMotor;
+	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+	{
+		m_invertedMotors[i] = 1;
+	}
+	m_deleteSpeedControllers = false;
+}
+
+/**
+ * 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 = frontLeftMotor;
+	m_rearLeftMotor = rearLeftMotor;
+	m_frontRightMotor = frontRightMotor;
+	m_rearRightMotor = rearRightMotor;
+	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+	{
+		m_invertedMotors[i] = 1;
+	}
+	m_deleteSpeedControllers = false;
+}
+
+RobotDrive::RobotDrive(SpeedController &frontLeftMotor, SpeedController &rearLeftMotor,
+						SpeedController &frontRightMotor, SpeedController &rearRightMotor)
+{
+	InitRobotDrive();
+	m_frontLeftMotor = &frontLeftMotor;
+	m_rearLeftMotor = &rearLeftMotor;
+	m_frontRightMotor = &frontRightMotor;
+	m_rearRightMotor = &rearRightMotor;
+	for (int32_t i=0; i < kMaxNumberOfMotors; i++)
+	{
+		m_invertedMotors[i] = 1;
+	}
+	m_deleteSpeedControllers = false;
+}
+
+/**
+ * RobotDrive destructor.
+ * Deletes motor objects that were not passed in and created internally only.
+ **/
+RobotDrive::~RobotDrive()
+{
+	if (m_deleteSpeedControllers)
+	{
+		delete m_frontLeftMotor;
+		delete m_rearLeftMotor;
+		delete m_frontRightMotor;
+		delete m_rearRightMotor;
+	}
+	// FIXME: delete m_safetyHelper;
+}
+
+/**
+ * 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(float outputMagnitude, float curve)
+{
+	float leftOutput, rightOutput;
+	static bool reported = false;
+	if (!reported)
+	{
+		reported = true;
+	}
+
+	if (curve < 0)
+	{
+		float value = log(-curve);
+		float ratio = (value - m_sensitivity)/(value + m_sensitivity);
+		if (ratio == 0) ratio =.0000000001;
+		leftOutput = outputMagnitude / ratio;
+		rightOutput = outputMagnitude;
+	}
+	else if (curve > 0)
+	{
+		float value = log(curve);
+		float 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, uint32_t leftAxis,
+		GenericHID *rightStick, uint32_t 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, uint32_t leftAxis,
+		GenericHID &rightStick, uint32_t 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(float leftValue, float 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 fowards/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, uint32_t moveAxis,
+								GenericHID* rotateStick, uint32_t rotateAxis,
+								bool squaredInputs)
+{
+	float moveValue = moveStick->GetRawAxis(moveAxis);
+	float 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 fowards/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, uint32_t moveAxis,
+								GenericHID &rotateStick, uint32_t rotateAxis,
+								bool squaredInputs)
+{
+	float moveValue = moveStick.GetRawAxis(moveAxis);
+	float 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(float moveValue, float rotateValue, bool squaredInputs)
+{
+	static bool reported = false;
+	if (!reported)
+	{
+		reported = true;
+	}
+
+	// local variables to hold the computed PWM values for the motors
+	float leftMotorOutput;
+	float 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(float x, float y, float rotation, float 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);
+
+	uint8_t syncGroup = 0x80;
+
+	m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
+	m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
+	m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
+	m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
+
+	// CANJaguar::UpdateSyncGroup(syncGroup);
+
+	// 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(float magnitude, float direction, float rotation)
+{
+	static bool reported = false;
+	if (!reported)
+	{
+		reported = true;
+	}
+
+	// Normalized for full power along the Cartesian axes.
+	magnitude = Limit(magnitude) * sqrt(2.0);
+	// The rollers are at 45 degree angles.
+	double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
+	double cosD = cos(dirInRad);
+	double sinD = 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);
+
+	uint8_t syncGroup = 0x80;
+
+	m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
+	m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
+	m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
+	m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
+
+	// CANJaguar::UpdateSyncGroup(syncGroup);
+
+	// 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 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::HolonomicDrive(float magnitude, float direction, float 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(float leftOutput, float rightOutput)
+{
+	wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
+
+	uint8_t syncGroup = 0x80;
+
+	if (m_frontLeftMotor != nullptr)
+		m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
+	m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
+
+	if (m_frontRightMotor != nullptr)
+		m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
+	m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
+
+	// CANJaguar::UpdateSyncGroup(syncGroup);
+
+	// FIXME: m_safetyHelper->Feed();
+}
+
+/**
+ * Limit motor values to the -1.0 to +1.0 range.
+ */
+float RobotDrive::Limit(float 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 = fabs(wheelSpeeds[0]);
+	int32_t i;
+	for (i=1; i<kMaxNumberOfMotors; i++)
+	{
+		double temp = 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 = cos(angle * (3.14159 / 180.0));
+	double sinA = 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(float 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(float timeout)
+{
+	// FIXME: m_safetyHelper->SetExpiration(timeout);
+}
+
+float 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/simulation/src/SafePWM.cpp b/wpilibc/simulation/src/SafePWM.cpp
new file mode 100644
index 0000000..8ba0b68
--- /dev/null
+++ b/wpilibc/simulation/src/SafePWM.cpp
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "SafePWM.h"
+#include <sstream>
+#include <memory>
+
+/**
+ * Constructor for a SafePWM object taking a channel number.
+ * @param channel The PWM channel number (0..19).
+ */
+SafePWM::SafePWM(uint32_t 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(float timeout)
+{
+	m_safetyHelper->SetExpiration(timeout);
+}
+
+/**
+ * Return the expiration time for the PWM object.
+ * @returns The expiration time value.
+ */
+float SafePWM::GetExpiration() const
+{
+	return m_safetyHelper->GetExpiration();
+}
+
+/**
+ * Check if the PWM object is currently alive or stopped due to a timeout.
+ * @returns 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(float speed)
+{
+	PWM::SetSpeed(speed);
+	m_safetyHelper->Feed();
+}
diff --git a/wpilibc/simulation/src/SampleRobot.cpp b/wpilibc/simulation/src/SampleRobot.cpp
new file mode 100644
index 0000000..b8f3181
--- /dev/null
+++ b/wpilibc/simulation/src/SampleRobot.cpp
@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "SampleRobot.h"
+
+#include <stdio.h>
+#include "Timer.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include "LiveWindow/LiveWindow.h"
+#include "networktables/NetworkTable.h"
+
+#if defined(_UNIX)
+	#include <unistd.h>
+#elif defined(_WIN32)
+  #include <windows.h>
+  void sleep(unsigned milliseconds)
+	    {
+	        Sleep(milliseconds);
+	    }
+#endif
+
+
+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()
+{
+	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()
+{
+	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()
+{
+	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()
+{
+	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()
+{
+	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();
+
+	SmartDashboard::init();
+	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/simulation/src/SensorBase.cpp b/wpilibc/simulation/src/SensorBase.cpp
new file mode 100644
index 0000000..fa3086f
--- /dev/null
+++ b/wpilibc/simulation/src/SensorBase.cpp
@@ -0,0 +1,161 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "SensorBase.h"
+
+#include "WPIErrors.h"
+
+const uint32_t SensorBase::kDigitalChannels;
+const uint32_t SensorBase::kAnalogInputs;
+const uint32_t SensorBase::kSolenoidChannels;
+const uint32_t SensorBase::kSolenoidModules;
+const uint32_t SensorBase::kPwmChannels;
+const uint32_t SensorBase::kRelayChannels;
+const uint32_t SensorBase::kPDPChannels;
+const uint32_t SensorBase::kChassisSlots;
+SensorBase *SensorBase::m_singletonList = nullptr;
+
+/**
+ * Creates an instance of the sensor base and gets an FPGA handle
+ */
+SensorBase::SensorBase()
+{
+}
+
+/**
+ * Add sensor to the singleton list.
+ * Add this sensor to the list of singletons that need to be deleted when
+ * the robot program exits. Each of the sensors on this list are singletons,
+ * that is they aren't allocated directly with new, but instead are allocated
+ * by the static GetInstance method. As a result, they are never deleted when
+ * the program exits. Consequently these sensors may still be holding onto
+ * resources and need to have their destructors called at the end of the program.
+ */
+void SensorBase::AddToSingletonList()
+{
+	m_nextSingleton = m_singletonList;
+	m_singletonList = this;
+}
+
+/**
+ * Delete all the singleton classes on the list.
+ * All the classes that were allocated as singletons need to be deleted so
+ * their resources can be freed.
+ */
+void SensorBase::DeleteSingletons()
+{
+	for (SensorBase *next = m_singletonList; next != nullptr;)
+	{
+		SensorBase *tmp = next;
+		next = next->m_nextSingleton;
+		delete tmp;
+	}
+	m_singletonList = nullptr;
+}
+
+/**
+ * Check that the solenoid module number is valid.
+ *
+ * @return Solenoid module is valid and present
+ */
+bool SensorBase::CheckSolenoidModule(uint8_t moduleNumber)
+{
+	return 1 <= moduleNumber && moduleNumber <= 2; // TODO: Fix for Athena
+}
+
+/**
+ * Check that the digital channel number is valid.
+ * Verify that the channel number is one of the legal channel numbers. Channel numbers are
+ * 1-based.
+ *
+ * @return Digital channel is valid
+ */
+bool SensorBase::CheckDigitalChannel(uint32_t 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
+ * 1-based.
+ *
+ * @return Relay channel is valid
+ */
+bool SensorBase::CheckRelayChannel(uint32_t 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
+ * 1-based.
+ *
+ * @return PWM channel is valid
+ */
+bool SensorBase::CheckPWMChannel(uint32_t 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 1-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogInput(uint32_t 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 1-based.
+ *
+ * @return Analog channel is valid
+ */
+bool SensorBase::CheckAnalogOutput(uint32_t 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(uint32_t 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(uint32_t channel)
+{
+	if (channel > 0 && channel <= kPDPChannels)
+		return true;
+	return false;
+}
diff --git a/wpilibc/simulation/src/Solenoid.cpp b/wpilibc/simulation/src/Solenoid.cpp
new file mode 100644
index 0000000..387194d
--- /dev/null
+++ b/wpilibc/simulation/src/Solenoid.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Solenoid.h"
+#include "WPIErrors.h"
+#include "LiveWindow/LiveWindow.h"
+#include "simulation/simTime.h"
+
+/**
+ * Constructor.
+ *
+ * @param channel The channel on the solenoid module to control (1..8).
+ */
+Solenoid::Solenoid(uint32_t 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(uint8_t moduleNumber, uint32_t channel)
+{
+    char buffer[50];
+    int n = sprintf(buffer, "pneumatic/%d/%d", moduleNumber, channel);
+    m_impl = new SimContinuousOutput(buffer);
+  
+	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/simulation/src/Talon.cpp b/wpilibc/simulation/src/Talon.cpp
new file mode 100644
index 0000000..0e9505d
--- /dev/null
+++ b/wpilibc/simulation/src/Talon.cpp
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Talon.h"
+
+//#include "NetworkCommunication/UsageReporting.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * @param channel The PWM channel that the Talon is attached to.
+ */
+Talon::Talon(uint32_t 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.
+ * @param syncGroup Unused interface.
+ */
+void Talon::Set(float speed, uint8_t syncGroup)
+{
+	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.
+ */
+float 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(float output)
+{
+	Set(output);
+}
diff --git a/wpilibc/simulation/src/Timer.cpp b/wpilibc/simulation/src/Timer.cpp
new file mode 100644
index 0000000..b4734db
--- /dev/null
+++ b/wpilibc/simulation/src/Timer.cpp
@@ -0,0 +1,208 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Timer.h"
+
+#include <time.h>
+
+#include "simulation/simTime.h"
+#include "Utility.h"
+
+
+// Internal stuff
+#include "simulation/SimFloatInput.h"
+#include "simulation/MainNode.h"
+namespace wpilib { namespace internal {
+    double simTime = 0;
+    std::condition_variable time_wait;
+    std::mutex time_wait_mutex;
+
+    void time_callback(const msgs::ConstFloat64Ptr &msg) {
+        simTime = msg->data();
+        time_wait.notify_all();
+    }
+}}
+
+/**
+ * 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
+}
+
+//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 unsigned 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.
+ * @returns 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();
+}
+
+// Internal function that reads the PPC timestamp counter.
+extern "C"
+{
+	uint32_t niTimestamp32(void);
+	uint64_t niTimestamp64(void);
+}
diff --git a/wpilibc/simulation/src/Utility.cpp b/wpilibc/simulation/src/Utility.cpp
new file mode 100644
index 0000000..b0710f1
--- /dev/null
+++ b/wpilibc/simulation/src/Utility.cpp
@@ -0,0 +1,235 @@
+
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Utility.h"
+
+#include "Timer.h"
+#include "simulation/simTime.h"
+#include <stdio.h>
+#include <string.h>
+
+#include <iostream>
+#include <sstream>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#if defined(UNIX)
+	#include <execinfo.h>
+	#include <cxxabi.h>
+#endif
+
+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)
+	// {
+	// 	printf("\n-----------<Stack Trace>----------------\n");
+	// 	printCurrentStackTrace();
+	// }
+	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, const std::string &conditionText,
+                     const std::string &message, const std::string &fileName,
+                     uint32_t lineNumber, const std::string &funcName) {
+  if (!conditionValue) {
+    // Error string buffer
+    std::stringstream error;
+
+    // If an error message was specified, include it
+    // Build error string
+    if (message.size()) {
+      error << "Assertion failed: \"" << message << "\", \"" << conditionText
+            << "\" failed in " << funcName + "() in " << fileName << " at line "
+            << lineNumber;
+    } else {
+      error << "Assertion failed: \"" << conditionText << "\" in " << funcName
+            << "() in " << fileName << " at line " << lineNumber;
+    }
+
+    // Print to console and send to remote dashboard
+    std::cout << "\n\n>>>>" << error.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,
+                                 const std::string &equalityType,
+                                 const std::string &message,
+                                 const std::string &fileName,
+                                 uint32_t lineNumber,
+                                 const std::string &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,
+						  const std::string &message,
+						  const std::string &fileName,
+						  uint32_t lineNumber,
+						  const std::string &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,
+						     const std::string &message,
+						     const std::string &fileName,
+						     uint32_t lineNumber,
+						     const std::string &funcName)
+{
+	if(!(valueA != valueB))
+	{
+		wpi_assertEqual_common_impl(valueA, valueB, "==", message, fileName, lineNumber, funcName);
+	}
+	return valueA != valueB;
+}
+
+/**
+ * Read the microsecond-resolution timer on the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA reset).
+ */
+uint32_t GetFPGATime()
+{
+	return wpilib::internal::simTime * 1e6;
+}
+
+//TODO: implement symbol demangling and backtrace on windows
+#if defined(UNIX)
+
+/**
+ * Demangle a C++ symbol, used for printing stack traces.
+ */
+static std::string demangle(char const *mangledSymbol)
+{
+	char buffer[256];
+	size_t length;
+	int status;
+
+	if(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(uint32_t 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;
+		}
+	}
+
+	free(mangledSymbols);
+
+	return trace.str();
+}
+
+#else
+static std::string demangle(char const *mangledSymbol)
+{
+	return "no demangling on windows";
+}
+std::string GetStackTrace(uint32_t offset)
+{
+	return "no stack trace on windows";
+}
+#endif
diff --git a/wpilibc/simulation/src/Victor.cpp b/wpilibc/simulation/src/Victor.cpp
new file mode 100644
index 0000000..54ba61f
--- /dev/null
+++ b/wpilibc/simulation/src/Victor.cpp
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Victor.h"
+
+//#include "NetworkCommunication/UsageReporting.h"
+#include "LiveWindow/LiveWindow.h"
+
+/**
+ * @param channel The PWM channel that the Victor is attached to.
+ */
+Victor::Victor(uint32_t 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.
+ * @param syncGroup Unused interface.
+ */
+void Victor::Set(float speed, uint8_t syncGroup)
+{
+	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.
+ */
+float 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(float output)
+{
+	Set(output);
+}
diff --git a/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp b/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp
new file mode 100644
index 0000000..39dbf70
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimContinuousOutput.cpp
@@ -0,0 +1,24 @@
+/*
+ * SimContinuousOutput.cpp
+ *
+ *  Created on: May 28, 2014
+ *      Author: alex
+ */
+
+#include "simulation/SimContinuousOutput.h"
+#include "simulation/MainNode.h"
+
+SimContinuousOutput::SimContinuousOutput(std::string topic) {
+    pub = MainNode::Advertise<msgs::Float64>("~/simulator/"+topic);
+	std::cout << "Initialized ~/simulator/"+topic << std::endl;
+}
+
+void SimContinuousOutput::Set(float speed) {
+	msgs::Float64 msg;
+	msg.set_data(speed);
+	pub->Publish(msg);
+}
+
+float SimContinuousOutput::Get() {
+	return speed;
+}
diff --git a/wpilibc/simulation/src/simulation/SimDigitalInput.cpp b/wpilibc/simulation/src/simulation/SimDigitalInput.cpp
new file mode 100644
index 0000000..b39f2ae
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimDigitalInput.cpp
@@ -0,0 +1,22 @@
+/*
+ * SimDigitalInput.cpp
+ *
+ *  Created on: May 28, 2014
+ *      Author: alex
+ */
+
+#include "simulation/SimDigitalInput.h"
+#include "simulation/MainNode.h"
+
+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 msgs::ConstBoolPtr &msg) {
+  value = msg->data();
+}
diff --git a/wpilibc/simulation/src/simulation/SimEncoder.cpp b/wpilibc/simulation/src/simulation/SimEncoder.cpp
new file mode 100644
index 0000000..b72b8f1
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimEncoder.cpp
@@ -0,0 +1,54 @@
+
+#include "simulation/SimEncoder.h"
+#include "simulation/MainNode.h"
+
+SimEncoder::SimEncoder(std::string topic) {
+	commandPub = MainNode::Advertise<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) {
+	msgs::GzString msg;
+	msg.set_data(cmd);
+	commandPub->Publish(msg);
+}
+
+
+void SimEncoder::positionCallback(const msgs::ConstFloat64Ptr &msg) {
+	position = msg->data();
+}
+
+void SimEncoder::velocityCallback(const msgs::ConstFloat64Ptr &msg) {
+	velocity = msg->data();
+}
diff --git a/wpilibc/simulation/src/simulation/SimFloatInput.cpp b/wpilibc/simulation/src/simulation/SimFloatInput.cpp
new file mode 100644
index 0000000..1fa9337
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimFloatInput.cpp
@@ -0,0 +1,22 @@
+/*
+ * SimFloatInput.cpp
+ *
+ *  Created on: May 28, 2014
+ *      Author: alex
+ */
+
+#include "simulation/SimFloatInput.h"
+#include "simulation/MainNode.h"
+
+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 msgs::ConstFloat64Ptr &msg) {
+  value = msg->data();
+}
diff --git a/wpilibc/simulation/src/simulation/SimGyro.cpp b/wpilibc/simulation/src/simulation/SimGyro.cpp
new file mode 100644
index 0000000..c173c05
--- /dev/null
+++ b/wpilibc/simulation/src/simulation/SimGyro.cpp
@@ -0,0 +1,46 @@
+
+#include "simulation/SimGyro.h"
+#include "simulation/MainNode.h"
+
+SimGyro::SimGyro(std::string topic) {
+    commandPub = MainNode::Advertise<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) {
+  msgs::GzString msg;
+  msg.set_data(cmd);
+  commandPub->Publish(msg);
+}
+
+
+void SimGyro::positionCallback(const msgs::ConstFloat64Ptr &msg) {
+    position = msg->data();
+}
+
+void SimGyro::velocityCallback(const msgs::ConstFloat64Ptr &msg) {
+    velocity = msg->data();
+}