diff --git a/CMakeLists.txt b/CMakeLists.txt
index daa9418f4f..484f86430c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,7 +48,7 @@ endif()
 if (MSVC)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DFRC_SIMULATOR /MDd /Zi")
 else ()
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++1y -DFRC_SIMULATOR -Wno-unused-parameter -pthread -fPIC -fpermissive")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -std=c++1y -DFRC_SIMULATOR -Wno-unused-parameter -pthread -fPIC -fpermissive")
 endif()
 
 
diff --git a/simulation/SimDS/build.gradle b/simulation/SimDS/build.gradle
index 0acaaee100..7d44b8e1b6 100644
--- a/simulation/SimDS/build.gradle
+++ b/simulation/SimDS/build.gradle
@@ -51,4 +51,6 @@ task simDsJavadoc(type: Jar, dependsOn: javadoc) {
     group = 'WPILib'
     classifier = 'javadoc'
     from javadoc.destinationDir
-}
\ No newline at end of file
+}
+
+build.dependsOn shadowJar
diff --git a/simulation/frcsim.sh b/simulation/frcsim
similarity index 79%
rename from simulation/frcsim.sh
rename to simulation/frcsim
index a923f53ccd..b5b2d73334 100755
--- a/simulation/frcsim.sh
+++ b/simulation/frcsim
@@ -1,5 +1,5 @@
 #!/bin/bash
 export GAZEBO_PLUGIN_PATH="${GAZEBO_PLUGIN_PATH}:${HOME}/wpilib/simulation/plugins"
 export GAZEBO_MODEL_PATH="${GAZEBO_MODEL_PATH}:${HOME}/wpilib/simulation/models"
-export LD_LIBRARY_PATH="${LD_LIBRARY_PATH}:${HOME}/wpilib/simulation/plugins"
-gazebo $@
+export LD_LIBRARY_PATH="${LD_LIBRARY_PATH}:${HOME}/wpilib/simulation/plugins:${HOME}/wpilib/simulation/lib"
+gazebo --verbose $@
diff --git a/simulation/gz_msgs/CMakeLists.txt b/simulation/gz_msgs/CMakeLists.txt
index 17ecf5cf8b..31c7dbaa5c 100644
--- a/simulation/gz_msgs/CMakeLists.txt
+++ b/simulation/gz_msgs/CMakeLists.txt
@@ -62,3 +62,4 @@ else()
 endif()
 
 target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARIES})
+set_target_properties(${PROJECT_NAME} PROPERTIES INSTALL_RPATH "${ENV_HOME}/wpilib/simulation/lib")
diff --git a/simulation/sim_ds b/simulation/sim_ds
new file mode 100755
index 0000000000..e9e1c52266
--- /dev/null
+++ b/simulation/sim_ds
@@ -0,0 +1 @@
+java -Djava.library.path=/home/peter/wpilib/simulation/jar -jar /home/peter/wpilib/simulation/jar/SimDS-all.jar
diff --git a/wpilibc/simulation/CMakeLists.txt b/wpilibc/simulation/CMakeLists.txt
index d7ee886683..09c92330fc 100644
--- a/wpilibc/simulation/CMakeLists.txt
+++ b/wpilibc/simulation/CMakeLists.txt
@@ -15,7 +15,6 @@ set (INCLUDE_FOLDERS include
   ${Boost_INCLUDE_DIR}
   ${GAZEBO_INCLUDE_DIRS})
 
-message("inc=${INCLUDE_FOLDERS}")
 
 include_directories(${INCLUDE_FOLDERS})
 
diff --git a/wpilibc/simulation/include/DriverStation.h b/wpilibc/simulation/include/DriverStation.h
index f15985cd11..f25c09391d 100644
--- a/wpilibc/simulation/include/DriverStation.h
+++ b/wpilibc/simulation/include/DriverStation.h
@@ -1,112 +1,142 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
+/* 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.  */
 /*----------------------------------------------------------------------------*/
 #pragma once
 
+#include "simulation/gz_msgs/msgs.h"
+
+#ifdef _WIN32
+  // Ensure that Winsock2.h is included before Windows.h, which can get
+  // pulled in by anybody (e.g., Boost).
+  #include <Winsock2.h>
+#endif
+
+#include <gazebo/transport/transport.hh>
 #include "SensorBase.h"
 #include "RobotState.h"
-#include "HAL/HAL.hpp"
-#include "HAL/cpp/Semaphore.hpp"
-#include "HAL/cpp/priority_mutex.h"
-#include "HAL/cpp/priority_condition_variable.h"
+#include <mutex>
 #include <condition_variable>
 
-struct HALControlWord;
+struct HALCommonControlData;
 class AnalogInput;
 
+using namespace gazebo;
+
 /**
- * Provide access to the network communication data to / from the Driver
- * Station.
+ * Provide access to the network communication data to / from the Driver Station.
  */
-class DriverStation : public SensorBase, public RobotStateInterface {
- public:
-  enum Alliance { kRed, kBlue, kInvalid };
+class DriverStation : public SensorBase, public RobotStateInterface
+{
+public:
+	enum Alliance
+	{
+		kRed,
+		kBlue,
+		kInvalid
+	};
 
-  virtual ~DriverStation();
-  static DriverStation &GetInstance();
-  static void ReportError(std::string error);
+	virtual ~DriverStation() = default;
+	static DriverStation &GetInstance();
+	static void ReportError(std::string error);
 
-  static const uint32_t kJoystickPorts = 6;
+	static const uint32_t kBatteryChannel = 7;
+	static const uint32_t kJoystickPorts = 4;
+	static const uint32_t kJoystickAxes = 6;
 
-  float GetStickAxis(uint32_t stick, uint32_t axis);
-  int GetStickPOV(uint32_t stick, uint32_t pov);
-  uint32_t GetStickButtons(uint32_t stick) const;
-  bool GetStickButton(uint32_t stick, uint8_t button);
+	float GetStickAxis(uint32_t stick, uint32_t axis);
+	bool GetStickButton(uint32_t stick, uint32_t button);
+	short GetStickButtons(uint32_t stick);
 
-  int GetStickAxisCount(uint32_t stick) const;
-  int GetStickPOVCount(uint32_t stick) const;
-  int GetStickButtonCount(uint32_t stick) const;
+	float GetAnalogIn(uint32_t channel);
+	bool GetDigitalIn(uint32_t channel);
+	void SetDigitalOut(uint32_t channel, bool value);
+	bool GetDigitalOut(uint32_t channel);
 
-  bool GetJoystickIsXbox(uint32_t stick) const;
-  int GetJoystickType(uint32_t stick) const;
-  std::string GetJoystickName(uint32_t stick) const;
-  int GetJoystickAxisType(uint32_t stick, uint8_t axis) const;
+	bool IsEnabled() const;
+	bool IsDisabled() const;
+	bool IsAutonomous() const;
+	bool IsOperatorControl() const;
+	bool IsTest() const;
+	bool IsFMSAttached() const;
 
-  bool IsEnabled() const override;
-  bool IsDisabled() const override;
-  bool IsAutonomous() const override;
-  bool IsOperatorControl() const override;
-  bool IsTest() const override;
-  bool IsDSAttached() const;
-  bool IsNewControlData() const;
-  bool IsFMSAttached() const;
-  bool IsSysActive() const;
-  bool IsSysBrownedOut() const;
+	uint32_t GetPacketNumber() const;
+	Alliance GetAlliance() const;
+	uint32_t GetLocation() const;
+	void WaitForData();
+	double GetMatchTime() const;
+	float GetBatteryVoltage() const;
+	uint16_t GetTeamNumber() const;
 
-  Alliance GetAlliance() const;
-  uint32_t GetLocation() const;
-  void WaitForData();
-  double GetMatchTime() const;
-  float GetBatteryVoltage() const;
 
-  /** Only to be used to tell the Driver Station what code you claim to be
-   * executing
-   *   for diagnostic purposes only
-   * @param entering If true, starting disabled code; if false, leaving disabled
-   * code */
-  void InDisabled(bool entering) { m_userInDisabled = entering; }
-  /** Only to be used to tell the Driver Station what code you claim to be
-   * executing
-   *   for diagnostic purposes only
-   * @param entering If true, starting autonomous code; if false, leaving
-   * autonomous code */
-  void InAutonomous(bool entering) { m_userInAutonomous = entering; }
-  /** Only to be used to tell the Driver Station what code you claim to be
-   * executing
-   *   for diagnostic purposes only
-   * @param entering If true, starting teleop code; if false, leaving teleop
-   * code */
-  void InOperatorControl(bool entering) { m_userInTeleop = entering; }
-  /** Only to be used to tell the Driver Station what code you claim to be
-   * executing
-   *   for diagnostic purposes only
-   * @param entering If true, starting test code; if false, leaving test code */
-  void InTest(bool entering) { m_userInTest = entering; }
 
- protected:
-  DriverStation();
+	void IncrementUpdateNumber()
+	{
+		m_updateNumber++;
+	}
 
-  void GetData();
+	/** Only to be used to tell the Driver Station what code you claim to be executing
+	 *   for diagnostic purposes only
+	 * @param entering If true, starting disabled code; if false, leaving disabled code */
+	void InDisabled(bool entering)
+	{
+		m_userInDisabled = entering;
+	}
+	/** Only to be used to tell the Driver Station what code you claim to be executing
+	 *   for diagnostic purposes only
+	 * @param entering If true, starting autonomous code; if false, leaving autonomous code */
+	void InAutonomous(bool entering)
+	{
+		m_userInAutonomous = entering;
+	}
+	/** Only to be used to tell the Driver Station what code you claim to be executing
+	*   for diagnostic purposes only
+	* @param entering If true, starting teleop code; if false, leaving teleop code */
+	void InOperatorControl(bool entering)
+	{
+		m_userInTeleop = entering;
+	}
+	/** Only to be used to tell the Driver Station what code you claim to be executing
+	*   for diagnostic purposes only
+	* @param entering If true, starting test code; if false, leaving test code */
+	void InTest(bool entering)
+	{
+		m_userInTest = entering;
+	}
 
- private:
-  static DriverStation *m_instance;
-  void ReportJoystickUnpluggedError(std::string message);
-  void Run();
+protected:
+	DriverStation();
 
-  HALJoystickAxes m_joystickAxes[kJoystickPorts];
-  HALJoystickPOVs m_joystickPOVs[kJoystickPorts];
-  HALJoystickButtons m_joystickButtons[kJoystickPorts];
-  HALJoystickDescriptor m_joystickDescriptor[kJoystickPorts];
-  mutable Semaphore m_newControlData{Semaphore::kEmpty};
-  mutable priority_condition_variable m_packetDataAvailableCond;
-  priority_mutex m_packetDataAvailableMutex;
-  std::condition_variable_any m_waitForDataCond;
-  priority_mutex m_waitForDataMutex;
-  bool m_userInDisabled = false;
-  bool m_userInAutonomous = false;
-  bool m_userInTeleop = false;
-  bool m_userInTest = false;
-  double m_nextMessageTime = 0;
+private:
+	static void InitTask(DriverStation *ds);
+	static DriverStation *m_instance;
+	static uint8_t m_updateNumber;
+	///< TODO: Get rid of this and use the semaphore signaling
+	static const float kUpdatePeriod;
+
+    void stateCallback(const msgs::ConstDriverStationPtr &msg);
+    void joystickCallback(const msgs::ConstFRCJoystickPtr &msg, int i);
+    void joystickCallback0(const msgs::ConstFRCJoystickPtr &msg);
+    void joystickCallback1(const msgs::ConstFRCJoystickPtr &msg);
+    void joystickCallback2(const msgs::ConstFRCJoystickPtr &msg);
+    void joystickCallback3(const msgs::ConstFRCJoystickPtr &msg);
+    void joystickCallback4(const msgs::ConstFRCJoystickPtr &msg);
+    void joystickCallback5(const msgs::ConstFRCJoystickPtr &msg);
+
+	uint8_t m_digitalOut = 0;
+	std::condition_variable m_waitForDataCond;
+	std::mutex m_waitForDataMutex;
+  mutable std::recursive_mutex m_stateMutex;
+	std::recursive_mutex m_joystickMutex;
+	double m_approxMatchTimeOffset = 0;
+	bool m_userInDisabled = false;
+	bool m_userInAutonomous = false;
+	bool m_userInTeleop = false;
+	bool m_userInTest = false;
+
+    transport::SubscriberPtr stateSub;
+    transport::SubscriberPtr joysticksSub[6];
+    msgs::DriverStationPtr state;
+    msgs::FRCJoystickPtr joysticks[6];
 };
diff --git a/wpilibc/simulation/include/Joystick.h b/wpilibc/simulation/include/Joystick.h
index 0922359bf3..9599f2f2bf 100644
--- a/wpilibc/simulation/include/Joystick.h
+++ b/wpilibc/simulation/include/Joystick.h
@@ -1,6 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. All Rights Reserved.
- */
+/* 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.  */
 /*----------------------------------------------------------------------------*/
@@ -8,9 +7,7 @@
 #ifndef JOYSTICK_H_
 #define JOYSTICK_H_
 
-#include <cstdint>
 #include <memory>
-#include <vector>
 #include "GenericHID.h"
 #include "ErrorBase.h"
 
@@ -18,101 +15,64 @@ class DriverStation;
 
 /**
  * Handle input from standard Joysticks connected to the Driver Station.
- * This class handles standard input that comes from the Driver Station. Each
- * time a value is requested
- * the most recent value is returned. There is a single class instance for each
- * joystick and the mapping
+ * This class handles standard input that comes from the Driver Station. Each time a value is requested
+ * the most recent value is returned. There is a single class instance for each joystick and the mapping
  * of ports to hardware buttons depends on the code in the driver station.
  */
-class Joystick : public GenericHID, public ErrorBase {
- public:
-  static const uint32_t kDefaultXAxis = 0;
-  static const uint32_t kDefaultYAxis = 1;
-  static const uint32_t kDefaultZAxis = 2;
-  static const uint32_t kDefaultTwistAxis = 2;
-  static const uint32_t kDefaultThrottleAxis = 3;
-  typedef enum {
-    kXAxis,
-    kYAxis,
-    kZAxis,
-    kTwistAxis,
-    kThrottleAxis,
-    kNumAxisTypes
-  } AxisType;
-  static const uint32_t kDefaultTriggerButton = 1;
-  static const uint32_t kDefaultTopButton = 2;
-  typedef enum { kTriggerButton, kTopButton, kNumButtonTypes } ButtonType;
-  typedef enum { kLeftRumble, kRightRumble } RumbleType;
-  typedef enum {
-    kUnknown = -1,
-    kXInputUnknown = 0,
-    kXInputGamepad = 1,
-    kXInputWheel = 2,
-    kXInputArcadeStick = 3,
-    kXInputFlightStick = 4,
-    kXInputDancePad = 5,
-    kXInputGuitar = 6,
-    kXInputGuitar2 = 7,
-    kXInputDrumKit = 8,
-    kXInputGuitar3 = 11,
-    kXInputArcadePad = 19,
-    kHIDJoystick = 20,
-    kHIDGamepad = 21,
-    kHIDDriving = 22,
-    kHIDFlight = 23,
-    kHID1stPerson = 24
-  } HIDType;
-  explicit Joystick(uint32_t port);
-  Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes);
-  virtual ~Joystick() = default;
+class Joystick : public GenericHID, public ErrorBase
+{
+public:
+	static const uint32_t kDefaultXAxis = 1;
+	static const uint32_t kDefaultYAxis = 2;
+	static const uint32_t kDefaultZAxis = 3;
+	static const uint32_t kDefaultTwistAxis = 4;
+	static const uint32_t kDefaultThrottleAxis = 3;
+	typedef enum
+	{
+		kXAxis, kYAxis, kZAxis, kTwistAxis, kThrottleAxis, kNumAxisTypes
+	} AxisType;
+	static const uint32_t kDefaultTriggerButton = 1;
+	static const uint32_t kDefaultTopButton = 2;
+	typedef enum
+	{
+		kTriggerButton, kTopButton, kNumButtonTypes
+	} ButtonType;
 
-  Joystick(const Joystick&) = delete;
-  Joystick& operator=(const Joystick&) = delete;
+	explicit Joystick(uint32_t port);
+	Joystick(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes);
+	virtual ~Joystick() = default;
 
-  uint32_t GetAxisChannel(AxisType axis) const;
-  void SetAxisChannel(AxisType axis, uint32_t channel);
+    Joystick(const Joystick&) = delete;
+    Joystick& operator=(const Joystick&) = delete;
 
-  virtual float GetX(JoystickHand hand = kRightHand) const override;
-  virtual float GetY(JoystickHand hand = kRightHand) const override;
-  virtual float GetZ() const override;
-  virtual float GetTwist() const override;
-  virtual float GetThrottle() const override;
-  virtual float GetAxis(AxisType axis) const;
-  float GetRawAxis(uint32_t axis) const override;
+	uint32_t GetAxisChannel(AxisType axis);
+	void SetAxisChannel(AxisType axis, uint32_t channel);
 
-  virtual bool GetTrigger(JoystickHand hand = kRightHand) const override;
-  virtual bool GetTop(JoystickHand hand = kRightHand) const override;
-  virtual bool GetBumper(JoystickHand hand = kRightHand) const override;
-  virtual bool GetRawButton(uint32_t button) const override;
-  virtual int GetPOV(uint32_t pov = 0) const override;
-  bool GetButton(ButtonType button) const;
-  static Joystick *GetStickForPort(uint32_t port);
+	virtual float GetX(JoystickHand hand = kRightHand) const override;
+	virtual float GetY(JoystickHand hand = kRightHand) const override;
+	virtual float GetZ() const override;
+	virtual float GetTwist() const override;
+	virtual float GetThrottle() const override;
+	virtual float GetAxis(AxisType axis) const;
+	float GetRawAxis(uint32_t axis) const override;
 
-  virtual float GetMagnitude() const;
-  virtual float GetDirectionRadians() const;
-  virtual float GetDirectionDegrees() const;
+	virtual bool GetTrigger(JoystickHand hand = kRightHand) const override;
+	virtual bool GetTop(JoystickHand hand = kRightHand) const override;
+	virtual bool GetBumper(JoystickHand hand = kRightHand) const override;
+	virtual bool GetRawButton(uint32_t button) const override;
+	virtual int GetPOV(uint32_t pov = 1) const override;
+	bool GetButton(ButtonType button) const;
+	static Joystick* GetStickForPort(uint32_t port);
 
-  bool GetIsXbox() const;
-  Joystick::HIDType GetType() const;
-  std::string GetName() const;
-  int GetAxisType(uint8_t axis) const;
+	virtual float GetMagnitude() const;
+	virtual float GetDirectionRadians() const;
+	virtual float GetDirectionDegrees() const;
 
-  int GetAxisCount() const;
-  int GetButtonCount() const;
-  int GetPOVCount() const;
-
-  void SetRumble(RumbleType type, float value);
-  void SetOutput(uint8_t outputNumber, bool value);
-  void SetOutputs(uint32_t value);
-
- private:
-  DriverStation &m_ds;
-  uint32_t m_port;
-  ::std::vector<uint32_t> m_axes;
-  ::std::vector<uint32_t> m_buttons;
-  uint32_t m_outputs = 0;
-  uint16_t m_leftRumble = 0;
-  uint16_t m_rightRumble = 0;
+private:
+	DriverStation &m_ds;
+	uint32_t m_port;
+	std::unique_ptr<uint32_t[]> m_axes;
+	std::unique_ptr<uint32_t[]> m_buttons;
 };
 
 #endif
diff --git a/wpilibc/simulation/include/RobotBase.h b/wpilibc/simulation/include/RobotBase.h
index a88dbb2f29..15a96481bf 100644
--- a/wpilibc/simulation/include/RobotBase.h
+++ b/wpilibc/simulation/include/RobotBase.h
@@ -47,6 +47,7 @@ protected:
     RobotBase& operator=(const RobotBase&) = delete;
 
 	DriverStation &m_ds;
+  transport::SubscriberPtr time_sub;
 
 private:
 	static RobotBase *m_instance;
diff --git a/wpilibc/simulation/include/simulation/MainNode.h b/wpilibc/simulation/include/simulation/MainNode.h
index a76ef50632..f101d60f70 100644
--- a/wpilibc/simulation/include/simulation/MainNode.h
+++ b/wpilibc/simulation/include/simulation/MainNode.h
@@ -39,10 +39,16 @@ public:
 	transport::NodePtr main;
 private:
 	MainNode() {
-		gazebo::client::setup();
-		main = transport::NodePtr(new transport::Node());
-		main->Init("frc");
-		gazebo::transport::run();
+		bool success = gazebo::client::setup();
+
+    if (success){
+  		main = transport::NodePtr(new transport::Node());
+  		main->Init("frc");
+   		gazebo::transport::run();
+    }
+    else {
+      std::cout << "An error has occured setting up gazebo_client!" << std::endl;
+    }
 	}
 };
 
diff --git a/wpilibc/simulation/src/DriverStation.cpp b/wpilibc/simulation/src/DriverStation.cpp
index 589ada7caa..607e8f5af0 100644
--- a/wpilibc/simulation/src/DriverStation.cpp
+++ b/wpilibc/simulation/src/DriverStation.cpp
@@ -1,227 +1,81 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. All Rights Reserved.
- */
+/* 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 "simulation/MainNode.h"
+//#include "MotorSafetyHelper.h"
 #include "Utility.h"
 #include "WPIErrors.h"
 #include <string.h>
 #include "Log.hpp"
+#include "boost/mem_fn.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)
+    if (level > dsLogLevel) ; \
+    else Log().Get(level)
 
+const uint32_t DriverStation::kBatteryChannel;
 const uint32_t DriverStation::kJoystickPorts;
+const uint32_t DriverStation::kJoystickAxes;
+const float DriverStation::kUpdatePeriod = 0.02;
+uint8_t DriverStation::m_updateNumber = 0;
 
 /**
- * DriverStation constructor.
+ * DriverStation contructor.
  *
  * 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);
+	state = msgs::DriverStationPtr(new msgs::DriverStation());
+	stateSub = MainNode::Subscribe("~/ds/state",
+		                   &DriverStation::stateCallback, this);
+	// TODO: for loop + boost bind
+	joysticks[0] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+	joysticksSub[0] =  MainNode::Subscribe("~/ds/joysticks/0",
+		                           &DriverStation::joystickCallback0, this);
+	joysticks[1] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+	joysticksSub[1] =  MainNode::Subscribe("~/ds/joysticks/1",
+		                           &DriverStation::joystickCallback1, this);
+	joysticks[2] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+	joysticksSub[2] =  MainNode::Subscribe("~/ds/joysticks/2",
+		                           &DriverStation::joystickCallback2, this);
+	joysticks[3] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+	joysticksSub[3] =  MainNode::Subscribe("~/ds/joysticks/5",
+	                                   &DriverStation::joystickCallback3, this);
+	joysticks[4] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+	joysticksSub[4] =  MainNode::Subscribe("~/ds/joysticks/4",
+	                                   &DriverStation::joystickCallback4, this);
+	joysticks[5] = msgs::FRCJoystickPtr(new msgs::FRCJoystick());
+	joysticksSub[5] =  MainNode::Subscribe("~/ds/joysticks/5",
+	                                   &DriverStation::joystickCallback5, this);
 
-  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();
-  }
+	AddToSingletonList();
 }
 
 /**
- * Return a reference to the singleton DriverStation.
- * @return Pointer to the DS instance
+ * Return a pointer to the singleton DriverStation.
  */
-DriverStation &DriverStation::GetInstance() {
-  static DriverStation instance;
-  return 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.
+ * Read the battery voltage. Hardcoded to 12 volts for Simulation.
  *
- * @return The battery voltage in Volts.
+ * @return The battery voltage.
  */
-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;
+float DriverStation::GetBatteryVoltage() const
+{
+	return 12.0; // 12 volts all the time!
 }
 
 /**
@@ -232,287 +86,285 @@ int DriverStation::GetStickButtonCount(uint32_t stick) const {
  * @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;
-  }
+float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis)
+{
+	if (axis < 0 || axis > (kJoystickAxes - 1))
+	{
+		wpi_setWPIError(BadJoystickAxis);
+		return 0.0;
+	}
+	if (stick < 0 || stick > 5)
+	{
+		wpi_setWPIError(BadJoystickIndex);
+		return 0.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;
-  }
+	std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+	if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size())
+	{
+		return 0.0;
+	}
+	return joysticks[stick]->axes(axis);
 }
 
 /**
- * Get the state of a POV on the joystick.
+ * The state of a specific button (1 - 12) on the joystick.
+ * This method only works in simulation, but is more efficient than GetStickButtons.
  *
- * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+ * @param stick The joystick to read.
+ * @param button The button number to check.
+ * @return If the button is pressed.
  */
-int DriverStation::GetStickPOV(uint32_t stick, uint32_t pov) {
-  if (stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
-    return -1;
-  }
+bool DriverStation::GetStickButton(uint32_t stick, uint32_t button)
+{
+    if (stick < 0 || stick >= 6)
+	{
+		wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5");
+		return false;
+	}
 
-  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];
+	std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+	if (joysticks[stick] == nullptr || button >= joysticks[stick]->buttons().size())
+	{
+		return false;
+	}
+	return joysticks[stick]->buttons(button-1);
 }
 
 /**
  * The state of the buttons on the joystick.
+ * 12 buttons (4 msb are unused) from the joystick.
  *
  * @param stick The joystick to read.
  * @return The state of the buttons on the joystick.
  */
-uint32_t DriverStation::GetStickButtons(uint32_t stick) const {
-  if (stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
-    return 0;
-  }
+short DriverStation::GetStickButtons(uint32_t stick)
+{
+    if (stick < 0 || stick >= 6)
+	{
+		wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5");
+		return false;
+	}
+	short btns = 0, btnid;
 
-  return m_joystickButtons[stick].buttons;
+	std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+	msgs::FRCJoystickPtr joy = joysticks[stick];
+	for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++)
+	{
+		if (joysticks[stick]->buttons(btnid))
+		{
+			btns |= (1 << btnid);
+		}
+	}
+	return btns;
 }
 
+// 5V divided by 10 bits
+#define kDSAnalogInScaling ((float)(5.0 / 1023.0))
+
 /**
- * The state of one joystick button. Button indexes begin at 1.
+ * Get an analog voltage from the Driver Station.
+ * The analog values are returned as voltage values for the Driver Station analog inputs.
+ * These inputs are typically used for advanced operator interfaces consisting of potentiometers
+ * or resistor networks representing values on a rotary switch.
  *
- * @param stick The joystick to read.
- * @param button The button index, beginning at 1.
- * @return The state of the joystick button.
+ * @param channel The analog input channel on the driver station to read from. Valid range is 1 - 4.
+ * @return The analog voltage on the input.
  */
-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;
+float DriverStation::GetAnalogIn(uint32_t channel)
+{
+	wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetAnalogIn");
+	return 0.0;
 }
 
 /**
- * Check if the DS has enabled the robot
- * @return True if the robot is enabled and the DS is connected
+ * Get values from the digital inputs on the Driver Station.
+ * Return digital values from the Drivers Station. These values are typically used for buttons
+ * and switches on advanced operator interfaces.
+ * @param channel The digital input to get. Valid range is 1 - 8.
  */
-bool DriverStation::IsEnabled() const {
-  HALControlWord controlWord;
-  memset(&controlWord, 0, sizeof(controlWord));
-  HALGetControlWord(&controlWord);
-  return controlWord.enabled && controlWord.dsAttached;
+bool DriverStation::GetDigitalIn(uint32_t channel)
+{
+	wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalIn");
+	return false;
 }
 
 /**
- * Check if the robot is disabled
- * @return True if the robot is explicitly disabled or the DS is not connected
+ * Set a value for the digital outputs on the Driver Station.
+ *
+ * Control digital outputs on the Drivers Station. These values are typically used for
+ * giving feedback on a custom operator station such as LEDs.
+ *
+ * @param channel The digital output to set. Valid range is 1 - 8.
+ * @param value The state to set the digital output.
  */
-bool DriverStation::IsDisabled() const {
-  HALControlWord controlWord;
-  memset(&controlWord, 0, sizeof(controlWord));
-  HALGetControlWord(&controlWord);
-  return !(controlWord.enabled && controlWord.dsAttached);
+void DriverStation::SetDigitalOut(uint32_t channel, bool value)
+{
+    wpi_setWPIErrorWithContext(UnsupportedInSimulation, "SetDigitalOut");
 }
 
 /**
- * Check if the DS is commanding autonomous mode
- * @return True if the robot is being commanded to be in autonomous mode
+ * Get a value that was set for the digital outputs on the Driver Station.
+ * @param channel The digital ouput to monitor. Valid range is 1 through 8.
+ * @return A digital value being output on the Drivers Station.
  */
-bool DriverStation::IsAutonomous() const {
-  HALControlWord controlWord;
-  memset(&controlWord, 0, sizeof(controlWord));
-  HALGetControlWord(&controlWord);
-  return controlWord.autonomous;
+bool DriverStation::GetDigitalOut(uint32_t channel)
+{
+	wpi_setWPIErrorWithContext(UnsupportedInSimulation, "GetDigitalOut");
+	return false;
 }
 
-/**
- * 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);
+bool DriverStation::IsEnabled() const
+{
+	std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+    return state != nullptr ? state->enabled() : false;
 }
 
-/**
- * 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;
+bool DriverStation::IsDisabled() const
+{
+    return !IsEnabled();
 }
 
-/**
- * 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;
+bool DriverStation::IsAutonomous() const
+{
+	std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+    return state != nullptr ?
+      state->state() == msgs::DriverStation_State_AUTO : false;
 }
 
-/**
- * @return always true in simulation
- */
-bool DriverStation::IsSysActive() const {
-  return true;
+bool DriverStation::IsOperatorControl() const
+{
+    return !(IsAutonomous() || IsTest());
 }
 
-/**
- * @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;
+bool DriverStation::IsTest() const
+{
+	std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+    return state != nullptr ?
+      state->state() == msgs::DriverStation_State_TEST : false;
 }
 
 /**
  * Is the driver station attached to a Field Management System?
- * @return True if the robot is competing on a field being controlled by a Field
- * Management System
+ * Note: This does not work with the Blue DS.
+ * @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;
+bool DriverStation::IsFMSAttached() const
+{
+    return false; // No FMS in simulation
 }
 
 /**
  * Return the alliance that the driver station says it is on.
  * This could return kRed or kBlue
- * @return The Alliance enum (kRed, kBlue or kInvalid)
+ * @return The Alliance enum
  */
-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;
-  }
+DriverStation::Alliance DriverStation::GetAlliance() const
+{
+	// if (m_controlData->dsID_Alliance == 'R') return kRed;
+	// if (m_controlData->dsID_Alliance == 'B') return kBlue;
+	// wpi_assert(false);
+    return kInvalid; // TODO: Support alliance colors
 }
 
 /**
  * Return the driver station location on the field
  * This could return 1, 2, or 3
- * @return The location of the driver station (1-3, 0 for invalid)
+ * @return The location of the driver station
  */
-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;
-  }
+uint32_t DriverStation::GetLocation() const
+{
+    return -1; // TODO: Support locations
 }
 
 /**
  * Wait until a new packet comes from the driver station
  * This blocks on a semaphore, so the waiting is efficient.
- * This is a good way to delay processing until there is new driver station data
- * to act on
+ * 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);
+void DriverStation::WaitForData()
+{
+	std::unique_lock<std::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)
+ * The FMS does not currently send the official match time to the robots
+ * This returns the time since the enable signal sent from the Driver Station
+ * At the beginning of autonomous, the time is reset to 0.0 seconds
+ * At the beginning of teleop, the time is reset to +15.0 seconds
+ * If the robot is disabled, this returns 0.0 seconds
+ * Warning: This is not an official time (so it cannot be used to argue with referees)
+ * @return Match time in seconds since the beginning of autonomous
  */
-double DriverStation::GetMatchTime() const {
-  float matchTime;
-  HALGetMatchTime(&matchTime);
-  return (double)matchTime;
+double DriverStation::GetMatchTime() const
+{
+	if (m_approxMatchTimeOffset < 0.0)
+		return 0.0;
+	return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset;
 }
 
 /**
  * Report an error to the DriverStation messages window.
  * The error is also printed to the program console.
  */
-void DriverStation::ReportError(std::string error) {
-  std::cout << error << std::endl;
-
-  HALControlWord controlWord;
-  HALGetControlWord(&controlWord);
-  if (controlWord.dsAttached) {
-    HALSetErrorData(error.c_str(), error.size(), 0);
-  }
+void DriverStation::ReportError(std::string error)
+{
+	std::cout << error << std::endl;
+}
+
+/**
+ * Return the team number that the Driver Station is configured for
+ * @return The team number
+ */
+uint16_t DriverStation::GetTeamNumber() const
+{
+    return 348;
+}
+
+void DriverStation::stateCallback(const msgs::ConstDriverStationPtr &msg)
+{
+    {
+  		std::unique_lock<std::recursive_mutex> lock(m_stateMutex);
+    	*state = *msg;
+  	}
+    m_waitForDataCond.notify_all();
+}
+
+void DriverStation::joystickCallback(const msgs::ConstFRCJoystickPtr &msg,
+                                     int i)
+{
+	std::unique_lock<std::recursive_mutex> lock(m_joystickMutex);
+    *(joysticks[i]) = *msg;
+}
+
+void DriverStation::joystickCallback0(const msgs::ConstFRCJoystickPtr &msg)
+{
+    joystickCallback(msg, 0);
+}
+
+void DriverStation::joystickCallback1(const msgs::ConstFRCJoystickPtr &msg)
+{
+    joystickCallback(msg, 1);
+}
+
+void DriverStation::joystickCallback2(const msgs::ConstFRCJoystickPtr &msg)
+{
+    joystickCallback(msg, 2);
+}
+
+void DriverStation::joystickCallback3(const msgs::ConstFRCJoystickPtr &msg)
+{
+    joystickCallback(msg, 3);
+}
+
+void DriverStation::joystickCallback4(const msgs::ConstFRCJoystickPtr &msg)
+{
+    joystickCallback(msg, 4);
+}
+
+void DriverStation::joystickCallback5(const msgs::ConstFRCJoystickPtr &msg)
+{
+    joystickCallback(msg, 5);
 }
diff --git a/wpilibc/simulation/src/Joystick.cpp b/wpilibc/simulation/src/Joystick.cpp
index a7a299ced8..d964e3cac5 100644
--- a/wpilibc/simulation/src/Joystick.cpp
+++ b/wpilibc/simulation/src/Joystick.cpp
@@ -1,16 +1,14 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. All Rights Reserved.
- */
+/* 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>
+#include <memory>
 
 const uint32_t Joystick::kDefaultXAxis;
 const uint32_t Joystick::kDefaultYAxis;
@@ -26,21 +24,19 @@ 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).
+ * @param port The port on the driver station that the joystick is plugged into.
  */
 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;
+	: 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);
+	m_buttons[kTriggerButton] = kDefaultTriggerButton;
+	m_buttons[kTopButton] = kDefaultTopButton;
 }
 
 /**
@@ -53,108 +49,111 @@ Joystick::Joystick(uint32_t port)
  * @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(uint32_t port, uint32_t numAxisTypes, uint32_t numButtonTypes)
+	: m_port (port),
+    m_ds(DriverStation::GetInstance())
+{
+	if ( !joySticksInitialized )
+	{
+		for (unsigned i = 0; i < DriverStation::kJoystickPorts; i++)
+			joysticks[i] = nullptr;
+		joySticksInitialized = true;
+	}
+	joysticks[m_port] = this;
+
+	m_axes = std::make_unique<uint32_t[]>(numAxisTypes);
+	m_buttons = std::make_unique<uint32_t[]>(numButtonTypes);
 }
 
-Joystick *Joystick::GetStickForPort(uint32_t port) {
-  Joystick *stick = joysticks[port];
-  if (stick == nullptr) {
-    stick = new Joystick(port);
-    joysticks[port] = stick;
-  }
-  return stick;
+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]);
+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]);
+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]); }
+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]); }
+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]);
+float Joystick::GetThrottle() const
+{
+	return GetRawAxis(m_axes[kThrottleAxis]);
 }
 
 /**
  * Get the value of the axis.
  *
- * @param axis The axis to read, starting at 0.
+ * @param axis The axis to read [1-6].
  * @return The value of the axis.
  */
-float Joystick::GetRawAxis(uint32_t axis) const {
-  return m_ds.GetStickAxis(m_port, 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
+ * 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;
-  }
+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;
+	}
 }
 
 /**
@@ -162,12 +161,12 @@ float Joystick::GetAxis(AxisType axis) const {
  *
  * 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.
+ * @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]);
+bool Joystick::GetTrigger(JoystickHand hand) const
+{
+	return GetRawButton(m_buttons[kTriggerButton]);
 }
 
 /**
@@ -175,45 +174,45 @@ bool Joystick::GetTrigger(JoystickHand hand) const {
  *
  * 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.
+ * @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]);
+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;
+bool Joystick::GetBumper(JoystickHand hand) const
+{
+	// Joysticks don't have bumpers.
+	return false;
 }
 
 /**
- * Get the button value (starting at button 1)
+ * Get the button value for buttons 1 through 12.
  *
- * The buttons are returned in a single 16 bit value with one bit representing
- * the state
+ * 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)
+ * @param button The button number to be read.
  * @return The state of the button.
  **/
-bool Joystick::GetRawButton(uint32_t button) const {
-  return m_ds.GetStickButton(m_port, 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.
- */
+* 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 Joystick::GetPOV(uint32_t pov) const {
-  return m_ds.GetStickPOV(m_port, pov);
+	return 0; // TODO
 }
 
 /**
@@ -224,75 +223,27 @@ int Joystick::GetPOV(uint32_t pov) const {
  * @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;
-  }
+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]; }
+uint32_t Joystick::GetAxisChannel(AxisType axis)
+{
+	return m_axes[axis];
+}
 
 /**
  * Set the channel associated with a specified axis.
@@ -300,8 +251,9 @@ uint32_t Joystick::GetAxisChannel(AxisType axis) const { return m_axes[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;
+void Joystick::SetAxisChannel(AxisType axis, uint32_t channel)
+{
+	m_axes[axis] = channel;
 }
 
 /**
@@ -311,7 +263,7 @@ void Joystick::SetAxisChannel(AxisType axis, uint32_t channel) {
  * @return The magnitude of the direction vector
  */
 float Joystick::GetMagnitude() const {
-  return sqrt(pow(GetX(), 2) + pow(GetY(), 2));
+	return sqrt(pow(GetX(),2) + pow(GetY(),2) );
 }
 
 /**
@@ -320,58 +272,19 @@ float Joystick::GetMagnitude() const {
  *
  * @return The direction of the vector in radians
  */
-float Joystick::GetDirectionRadians() const { return atan2(GetX(), -GetY()); }
+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
+ * uses acos(-1) to represent Pi due to absence of readily accessable 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);
+	return (180/acos(-1))*GetDirectionRadians();
 }
diff --git a/wpilibc/simulation/src/RobotBase.cpp b/wpilibc/simulation/src/RobotBase.cpp
index 1fa5ac74e5..c1a47a1cc5 100644
--- a/wpilibc/simulation/src/RobotBase.cpp
+++ b/wpilibc/simulation/src/RobotBase.cpp
@@ -34,7 +34,7 @@ RobotBase &RobotBase::getInstance()
 RobotBase::RobotBase() : m_ds(DriverStation::GetInstance())
 {
 	RobotState::SetImplementation(DriverStation::GetInstance());
-	transport::SubscriberPtr time_pub = MainNode::Subscribe("time", &wpilib::internal::time_callback);
+	time_sub = MainNode::Subscribe("~/time", &wpilib::internal::time_callback);
 }
 
 /**
diff --git a/wpilibc/simulation/src/Utility.cpp b/wpilibc/simulation/src/Utility.cpp
index b0710f14c2..7d12eb85c1 100644
--- a/wpilibc/simulation/src/Utility.cpp
+++ b/wpilibc/simulation/src/Utility.cpp
@@ -16,7 +16,7 @@
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
-#if defined(UNIX)
+#if not defined(_WIN32)
 	#include <execinfo.h>
 	#include <cxxabi.h>
 #endif
@@ -58,29 +58,27 @@ static void wpi_handleTracing()
  * 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) {
+bool wpi_assert_impl(bool conditionValue, const char *conditionText,
+                     const char *message, const char *fileName,
+                     uint32_t lineNumber, const char *funcName) {
   if (!conditionValue) {
-    // Error string buffer
-    std::stringstream error;
+    std::stringstream errorStream;
 
-    // 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;
+    errorStream << "Assertion \"" << conditionText << "\" ";
+    errorStream << "on line " << lineNumber << " ";
+    errorStream << "of " << basename(fileName) << " ";
+
+    if (message[0] != '\0') {
+      errorStream << "failed: " << message << std::endl;
     } else {
-      error << "Assertion failed: \"" << conditionText << "\" in " << funcName
-            << "() in " << fileName << " at line " << lineNumber;
+      errorStream << "failed." << std::endl;
     }
 
     // Print to console and send to remote dashboard
-    std::cout << "\n\n>>>>" << error.str();
-
+    std::cout << "\n\n>>>>" << errorStream.str();
     wpi_handleTracing();
   }
+
   return conditionValue;
 }
 
@@ -167,7 +165,7 @@ uint32_t GetFPGATime()
 }
 
 //TODO: implement symbol demangling and backtrace on windows
-#if defined(UNIX)
+#if not defined(_WIN32)
 
 /**
  * Demangle a C++ symbol, used for printing stack traces.
diff --git a/wpilibj/simulation.gradle b/wpilibj/simulation.gradle
index c276705188..543e866510 100644
--- a/wpilibj/simulation.gradle
+++ b/wpilibj/simulation.gradle
@@ -5,10 +5,25 @@ sourceSets {
 dependencies {
     simCompile sourceSets.shared.output
     simCompile project(':simulation:JavaGazebo')
-    simCompile 'edu.wpi.first.wpilib.networktables.java:NetworkTables:3.0.0-SNAPSHOT:arm'
+    simCompile 'edu.wpi.first.wpilib.networktables.java:NetworkTables:3.0.0-SNAPSHOT:desktop'
+    simRuntime "edu.wpi.first.wpilib.networktables.java:NetworkTables:3.0.0-SNAPSHOT:desktop"
 }
 
 task wpilibjSimJar(type: Jar, dependsOn: simClasses) {
+
+    def addClasspath = { classpath ->
+        classpath.files.findAll { it.exists() }.each {
+            if (file(it).directory) {
+                from it
+            } else {
+                from zipTree(it.path)
+            }
+        }
+    }
+
+    addClasspath sourceSets.shared.runtimeClasspath
+    addClasspath sourceSets.sim.runtimeClasspath
+
     description = 'Creates the WPILibJSimulation Jar'
     group = 'WPILib'
     from sourceSets.sim.output.classesDir
@@ -55,4 +70,6 @@ publishing {
             version '0.1.0-SNAPSHOT'
         }
     }
-}
\ No newline at end of file
+}
+
+build.dependsOn wpilibjSimJar