Allowed sharing of common C++ code between RoboRIO and Simulation.

Change-Id: I8bf2bda9df389c13ae0567a62dbf0ca931ceb6f8

CMakeLists.txt

@@ -5,6 +5,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wformat=2 -Wall -Wextra -Wno-unused-par
 SET(CMAKE_SKIP_BUILD_RPATH TRUE)
 
 file(GLOB_RECURSE NI_LIBS ni-libraries/*.so*)
+get_filename_component(WPILIB_INCLUDES wpilibc/wpilibC++/include REALPATH)
 get_filename_component(HAL_API_INCLUDES hal/include REALPATH)
 get_filename_component(NWT_API_INCLUDES networktables/cpp/include REALPATH)
 add_subdirectory(hal)

simulation/debs/Makefile

@@ -108,12 +108,12 @@ pull-eclipse-plugins:
 	echo Increment version?
 
 pull-eclipse-toolchain:
-	rm -rf frcsim-eclipse-plugins/frcsim-eclipse-toolchain-plugins/plugins
-	rm -rf frcsim-eclipse-plugins/frcsim-eclipse-toolchain-plugins/features
-	mkdir -p frcsim-eclipse-plugins/frcsim-eclipse-toolchain-plugins/plugins
-	mkdir -p frcsim-eclipse-plugins/frcsim-eclipse-toolchain-plugins/features
-	cp $(allwpilib)/eclipse-plugins/edu.wpi.first.wpilib.plugins.updatesite/target/site/plugins/edu.wpi.first.wpilib.plugins.cpp.toolchains.linux_* frcsim-eclipse-plugins/frcsim-eclipse-toolchain-plugins/plugins
-	cp $(allwpilib)/eclipse-plugins/edu.wpi.first.wpilib.plugins.updatesite/target/site/features/edu.wpi.first.wpilib.plugins.cpp.toolchains.linux.feature_* frcsim-eclipse-plugins/frcsim-eclipse-toolchain-plugins/features
+	rm -rf frcsim-eclipse-toolchain-plugin/frcsim-eclipse-toolchain-plugin/plugins
+	rm -rf frcsim-eclipse-toolchain-plugin/frcsim-eclipse-toolchain-plugin/features
+	mkdir -p frcsim-eclipse-toolchain-plugin/frcsim-eclipse-toolchain-plugin/plugins
+	mkdir -p frcsim-eclipse-toolchain-plugin/frcsim-eclipse-toolchain-plugin/features
+	cp $(allwpilib)/eclipse-plugins/edu.wpi.first.wpilib.plugins.updatesite/target/site/plugins/edu.wpi.first.wpilib.plugins.cpp.toolchains.linux_* frcsim-eclipse-toolchain-plugin/frcsim-eclipse-toolchain-plugin/plugins/
+	cp $(allwpilib)/eclipse-plugins/edu.wpi.first.wpilib.plugins.updatesite/target/site/features/edu.wpi.first.wpilib.plugins.cpp.toolchains.linux.feature_* frcsim-eclipse-toolchain-plugin/frcsim-eclipse-toolchain-plugin/features/
 	echo Increment version?
 
 pull-libwpilibsim-cpp:
@@ -125,6 +125,14 @@ download-models:
 	    wget $(gazebo-models-orig-url) -O frcsim-gazebo-models_${gazebo-models-version}.orig.tar.gz && \
 	    tar xvf frcsim-gazebo-models_${gazebo-models-version}.orig.tar.gz
 
+install:
+	sudo dpkg -i frcsim-gazebo-plugins/frcsim-gazebo-plugins_0.2-1_amd64.deb \
+                 frcsim-gazebo-models/frcsim-gazebo-models_0.2-1_all.deb \
+                 frcsim-eclipse-plugins/frcsim-eclipse-plugins_0.1-1_all.deb \
+                 frcsim-eclipse-toolchain-plugin/frcsim-eclipse-toolchain-plugin_0.1-1_all.deb \
+                 frcsim-libwpilibsim-cpp/frcsim-libwpilibsim-cpp_0.1-1_amd64.deb \
+                 frcsim/frcsim_0.1-1_all.deb
+
 deploy:
 	rsync -r -v -C -p repository/ adhenning@ccc.wpi.edu:public_html/frcsim/
 

simulation/debs/frcsim-gazebo-plugins/frcsim-gazebo-plugins/debian/control

@@ -3,11 +3,11 @@ Maintainer: Alex Henning <alex@thoriumrobotics.com>
 Section: misc
 Priority: optional
 Standards-Version: 3.9.4
-Build-Depends: debhelper (>= 9), gazebo3, libgazebo-dev, libavcodec-dev, libavformat-dev, libswscale-dev, libtinyxml2-dev
+Build-Depends: debhelper (>= 9), gazebo3, libgazebo-dev, libavcodec-dev, libavformat-dev, libswscale-dev
 
 Package: frcsim-gazebo-plugins
 Architecture: i386 amd64
-Depends: ${shlibs:Depends}, ${misc:Depends}, gazebo3, libgazebo-dev, libavcodec-dev, libswscale-dev, libtinyxml2-dev
+Depends: ${shlibs:Depends}, ${misc:Depends}, gazebo3, libgazebo-dev, libavcodec-dev, libswscale-dev
 Description: FRC plugin for controlling robots in the gazebo plugin.
  This plugin allows robots in gazebo to communicate with WPILib
  programs and enables the control of the robots.

wpilibc/CMakeLists.txt

@@ -2,4 +2,5 @@ cmake_minimum_required(VERSION 2.8)
 project(WPILibC)
 
 add_subdirectory(wpilibC++)
+add_subdirectory(wpilibC++Devices)
 add_subdirectory(wpilibC++IntegrationTests)

wpilibc/wpilibC++/CMakeLists.txt

@@ -1,11 +1,12 @@
 cmake_minimum_required(VERSION 2.8)
 project(WPILibC++)
 
-file(GLOB_RECURSE SRC_FILES lib/*.cpp)
-include_directories(include/ ${HAL_API_INCLUDES} ${NWT_API_INCLUDES})
-add_library(WPILibAthena STATIC ${SRC_FILES})
-target_link_libraries(WPILibAthena HALAthena NetworkTables ${NI_LIBS})
-INSTALL(TARGETS WPILibAthena ARCHIVE DESTINATION lib COMPONENT lib)
+file(GLOB_RECURSE SRC_FILES src/*.cpp)
+include_directories(include/ ${NWT_API_INCLUDES} ${HAL_API_INCLUDES})
+add_library(WPILib STATIC ${SRC_FILES})
+set(CMAKE_CXX_FLAGS "-fPIC")
+target_link_libraries(WPILib)
+INSTALL(TARGETS WPILib ARCHIVE DESTINATION src COMPONENT src)
 INSTALL(DIRECTORY include DESTINATION ${CMAKE_INSTALL_PREFIX} COMPONENT headers)
 # lib/ c m gcc_s ld-linux
 # usr/lib stdc++

wpilibc/wpilibC++/include/Controller.h

@@ -5,7 +5,10 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
-#include "HAL/HAL.hpp"
+#include <stdint.h>
+#include <cmath>
+#include <pthread.h>
+#include <semaphore.h>
 
 /**
  * Interface for Controllers

wpilibc/wpilibC++/include/Error.h

@@ -7,7 +7,7 @@
 
 #include "Base.h"
 #include <string>
-#include "HAL/HAL.hpp"
+#include <stdint.h>
 
 //  Forward declarations
 class ErrorBase;
@@ -49,5 +49,6 @@ private:
 	const ErrorBase* m_originatingObject;
 	double m_timestamp;
 
-	static bool m_suspendOnErrorEnabled;DISALLOW_COPY_AND_ASSIGN(Error);
+	static bool m_suspendOnErrorEnabled;
+    DISALLOW_COPY_AND_ASSIGN(Error);
 };

wpilibc/wpilibC++/include/ErrorBase.h

@@ -8,7 +8,6 @@
 #include "Base.h"
 #include "Error.h"
 #include "HAL/Semaphore.hpp"
-#include "HAL/HAL.hpp"
 
 #define wpi_setErrnoErrorWithContext(context)   (this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__))
 #define wpi_setErrnoError()   (wpi_setErrnoErrorWithContext(""))

wpilibc/wpilibC++/include/GenericHID.h

@@ -5,7 +5,7 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
-#include "HAL/HAL.hpp"
+#include <stdint.h>
 
 /** GenericHID Interface
  */

wpilibc/wpilibC++/include/HLUsageReporting.h

@@ -5,15 +5,21 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
-// If don't have C++11, define constexpr as const for WindRiver
-#if __cplusplus < 201103L
-#define constexpr const
-#define nullptr NULL
-#endif
-
-// A macro to disallow the copy constructor and operator= functions
-// This should be used in the private: declarations for a class
-#define DISALLOW_COPY_AND_ASSIGN(TypeName) \
-  TypeName(const TypeName&);               \
-  void operator=(const TypeName&)
+class HLUsageReportingInterface
+{
+public:
+	virtual ~HLUsageReportingInterface() {};
+	virtual void ReportScheduler() = 0;
+    virtual void ReportSmartDashboard() = 0;
+};
 
+class HLUsageReporting
+{
+private:
+	static HLUsageReportingInterface* impl;
+	
+public:
+	static void SetImplementation(HLUsageReportingInterface* i);
+	static void ReportScheduler();
+    static void ReportSmartDashboard();
+};

wpilibc/wpilibC++/include/Notifier.h

@@ -5,8 +5,8 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
-#include "HAL/HAL.hpp"
 #include "ErrorBase.h"
+#include "Task.h"
 #include "HAL/cpp/Synchronized.hpp"
 
 typedef void (*TimerEventHandler)(void *param);
@@ -22,7 +22,6 @@ public:
 private:
 	static Notifier *timerQueueHead;
 	static ReentrantSemaphore queueSemaphore;
-	static void* m_notifier;
 	static int refcount;
 
 	static void ProcessQueue(uint32_t mask, void *params); // process the timer queue on a timer event
@@ -39,4 +38,7 @@ private:
 	SEMAPHORE_ID m_handlerSemaphore;		// held by interrupt manager task while handler call is in progress
 
 	DISALLOW_COPY_AND_ASSIGN(Notifier);
+
+	static Task *task;
+	static void Run();
 };

wpilibc/wpilibC++/include/PIDController.h

@@ -69,7 +69,6 @@ private:
 	float m_minimumInput;		// minimum input - limit setpoint to this
 	bool m_continuous;	// do the endpoints wrap around? eg. Absolute encoder
 	bool m_enabled; 			//is the pid controller enabled
-	bool m_destruct; // should the calculate thread stop running
 	float m_prevError;	// the prior sensor input (used to compute velocity)
 	double m_totalError; //the sum of the errors for use in the integral calc
 	enum
@@ -83,7 +82,9 @@ private:
 	float m_error;
 	float m_result;
 	float m_period;
-
+	
+	MUTEX_ID m_semaphore;
+	
 	PIDSource *m_pidInput;
 	PIDOutput *m_pidOutput;
 
@@ -92,7 +93,7 @@ private:
 
 	void Initialize(float p, float i, float d, float f, PIDSource *source, PIDOutput *output,
 			float period = 0.05);
-	static void *CallCalculate(void *controller);
+	static void CallCalculate(void *controller);
 
 	virtual ITable* GetTable();
 	virtual std::string GetSmartDashboardType();

wpilibc/wpilibC++/include/Resource.h

@@ -7,7 +7,7 @@
 
 #include "ErrorBase.h"
 #include "HAL/cpp/Synchronized.hpp"
-#include "HAL/HAL.hpp"
+#include <stdint.h>
 
 /**
  * The Resource class is a convenient way to track allocated resources.

wpilibc/wpilibC++/include/RobotState.h

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* 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
+
+class RobotStateInterface
+{
+public:
+	virtual ~RobotStateInterface() {};
+    virtual bool IsDisabled() = 0;
+	virtual bool IsEnabled() = 0;
+	virtual bool IsOperatorControl() = 0;
+	virtual bool IsAutonomous() = 0;
+	virtual bool IsTest() = 0;
+};
+
+class RobotState
+{
+private:
+	static RobotStateInterface* impl;
+	
+public:
+	static void SetImplementation(RobotStateInterface* i);
+    static bool IsDisabled();
+	static bool IsEnabled();
+	static bool IsOperatorControl();
+	static bool IsAutonomous();
+	static bool IsTest();
+};
+
+

wpilibc/wpilibC++/include/Task.h

@@ -6,7 +6,7 @@
 #pragma once
 
 #include "ErrorBase.h"
-#include "HAL/HAL.hpp"
+#include "HAL/Task.hpp"
 
 /**
  * WPI task is a wrapper for the native Task object.

wpilibc/wpilibC++/include/Timer.h

@@ -34,6 +34,7 @@ public:
 
 	static double GetFPGATimestamp();
 	static double GetPPCTimestamp();
+    static double GetMatchTime();
 
 private:
 	double m_startTime;

wpilibc/wpilibC++/include/Utility.h

@@ -5,7 +5,7 @@
 /*---------------------------------------------------------------------------*/
 #pragma once
 
-#include "HAL/HAL.hpp"
+#include <stdint.h>
 #include <string>
 
 #define wpi_assert(condition) wpi_assert_impl(condition, #condition, NULL, __FILE__, __LINE__, __FUNCTION__)

wpilibc/wpilibC++/include/WPIErrors.h

@@ -54,6 +54,7 @@ S(NetworkTablesWrongType, -42, "The wrong type was read from the NetworkTables e
 S(NetworkTablesCorrupt, -43, "NetworkTables data stream is corrupt");
 S(SmartDashboardMissingKey, -43, "SmartDashboard data does not exist");
 S(CommandIllegalUse, -50, "Illegal use of Command");
+S(UnsupportedInSimulation, -80, "Unsupported in simulation");
 
 /*
  * Warnings

wpilibc/wpilibC++/lib/Buttons/Button.cpp

@@ -1,50 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/Button.h"
-
-/**
- * Specifies the command to run when a button is first pressed
- * @param command The pointer to the command to run
- */
-void Button::WhenPressed(Command *command) {
-	WhenActive(command);
-}
-
-/**
- * Specifies the command to be scheduled while the button is pressed
- * The command will be scheduled repeatedly while the button is pressed and will
- * be canceled when the button is released.
- * @param command The pointer to the command to run
- */
-void Button::WhileHeld(Command *command) {
-	WhileActive(command);
-}
-
-/**
- * Specifies the command to run when the button is released
- * The command will be scheduled a single time.
- * @param The pointer to the command to run
- */
-void Button::WhenReleased(Command *command) {
-	WhenInactive(command);
-}
-
-/**
- * Cancels the specificed command when the button is pressed
- * @param The command to be canceled
- */
-void Button::CancelWhenPressed(Command *command) {
-	CancelWhenActive(command);
-}
-
-/**
- * Toggle the specified command when the button is pressed
- * @param The command to be toggled
- */
-void Button::ToggleWhenPressed(Command *command) {
-	ToggleWhenActive(command);
-}

wpilibc/wpilibC++/lib/Buttons/ButtonScheduler.cpp

@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/ButtonScheduler.h"
-
-#include "Commands/Scheduler.h"
-
-ButtonScheduler::ButtonScheduler(bool last, Trigger *button, Command *orders) :
-	m_pressedLast(last),
-	m_button(button),
-	m_command(orders)
-{
-}
-
-void ButtonScheduler::Start()
-{
-	Scheduler::GetInstance()->AddButton(this);
-}

wpilibc/wpilibC++/lib/Buttons/CancelButtonScheduler.cpp

@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/CancelButtonScheduler.h"
-
-#include "Buttons/Button.h"
-#include "Commands/Command.h"
-
-CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger *button, Command *orders) :
-	ButtonScheduler(last, button, orders)
-{
-	pressedLast = m_button->Grab();
-}
-
-void CancelButtonScheduler::Execute()
-{
-    if (m_button->Grab()) {
-        if (!pressedLast) {
-            pressedLast = true;
-            m_command->Cancel();
-        }
-    } else {
-        pressedLast = false;
-    }
-}
-

wpilibc/wpilibC++/lib/Buttons/HeldButtonScheduler.cpp

@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/HeldButtonScheduler.h"
-
-#include "Buttons/Button.h"
-#include "Commands/Command.h"
-
-HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger *button, Command *orders) :
-	ButtonScheduler(last, button, orders)
-{
-}
-
-void HeldButtonScheduler::Execute()
-{
-	if (m_button->Grab())
-	{
-		m_pressedLast = true;
-		m_command->Start();
-	}
-	else
-	{
-		if (m_pressedLast)
-		{
-			m_pressedLast = false;
-			m_command->Cancel();
-		}
-	}
-}

wpilibc/wpilibC++/lib/Buttons/InternalButton.cpp

@@ -1,34 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/InternalButton.h"
-
-InternalButton::InternalButton() :
-	m_pressed(false),
-	m_inverted(false)
-{
-}
-
-InternalButton::InternalButton(bool inverted) :
-	m_pressed(inverted),
-	m_inverted(inverted)
-{
-}
-
-void InternalButton::SetInverted(bool inverted)
-{
-	m_inverted = inverted;
-}
-
-void InternalButton::SetPressed(bool pressed)
-{
-	m_pressed = pressed;
-}
-
-bool InternalButton::Get()
-{
-	return m_pressed ^ m_inverted;
-}

wpilibc/wpilibC++/lib/Buttons/JoystickButton.cpp

@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/JoystickButton.h"
-
-JoystickButton::JoystickButton(GenericHID *joystick, int buttonNumber) :
-	m_joystick(joystick),
-	m_buttonNumber(buttonNumber)
-{
-}
-
-bool JoystickButton::Get()
-{
-    return m_joystick->GetRawButton(m_buttonNumber);
-}

wpilibc/wpilibC++/lib/Buttons/NetworkButton.cpp

@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/NetworkButton.h"
-#include "networktables/NetworkTable.h"
-
-NetworkButton::NetworkButton(const char *tableName, const char *field) ://TODO how is this supposed to work???
-	m_netTable(NetworkTable::GetTable(tableName)),
-	m_field(field)
-{
-}
-
-NetworkButton::NetworkButton(ITable *table, const char *field) :
-	m_netTable(table),
-	m_field(field)
-{
-}
-
-bool NetworkButton::Get()
-{
-	/*if (m_netTable->isConnected())
-		return m_netTable->GetBoolean(m_field.c_str());
-	else
-		return false;*/
-	return m_netTable->GetBoolean(m_field);
-}

wpilibc/wpilibC++/lib/Buttons/PressedButtonScheduler.cpp

@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/PressedButtonScheduler.h"
-
-#include "Buttons/Button.h"
-#include "Commands/Command.h"
-
-PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger *button, Command *orders) :
-	ButtonScheduler(last, button, orders)
-{
-}
-
-void PressedButtonScheduler::Execute()
-{
-	if (m_button->Grab())
-	{
-		if (!m_pressedLast)
-		{
-			m_pressedLast = true;
-			m_command->Start();
-		}
-	}
-	else
-	{
-		m_pressedLast = false;
-	}
-}

wpilibc/wpilibC++/lib/Buttons/ReleasedButtonScheduler.cpp

@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/ReleasedButtonScheduler.h"
-
-#include "Buttons/Button.h"
-#include "Commands/Command.h"
-
-ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger *button, Command *orders) :
-	ButtonScheduler(last, button, orders)
-{
-}
-
-void ReleasedButtonScheduler::Execute()
-{
-	if (m_button->Grab())
-	{
-		m_pressedLast = true;
-	}
-	else
-	{
-		if (m_pressedLast)
-		{
-			m_pressedLast = false;
-			m_command->Start();
-		}
-	}
-}

wpilibc/wpilibC++/lib/Buttons/ToggleButtonScheduler.cpp

@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/ToggleButtonScheduler.h"
-
-#include "Buttons/Button.h"
-#include "Commands/Command.h"
-
-ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger *button,
-		Command *orders) :
-	ButtonScheduler(last, button, orders) {
-	pressedLast = m_button->Grab();
-}
-
-void ToggleButtonScheduler::Execute() {
-	if (m_button->Grab()) {
-		if (!pressedLast) {
-			pressedLast = true;
-			if (m_command->IsRunning()) {
-				m_command->Cancel();
-			} else {
-				m_command->Start();
-			}
-		}
-	} else {
-		pressedLast = false;
-	}
-}
-

wpilibc/wpilibC++/lib/Buttons/Trigger.cpp

@@ -1,75 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Buttons/Button.h"
-
-#include "Buttons/HeldButtonScheduler.h"
-#include "Buttons/PressedButtonScheduler.h"
-#include "Buttons/ReleasedButtonScheduler.h"
-#include "Buttons/ToggleButtonScheduler.h"
-#include "Buttons/CancelButtonScheduler.h"
-
-Trigger::Trigger() {
-	m_table = NULL;
-}
-
-bool Trigger::Grab()
-{
-	if (Get())
-		return true;
-	else if (m_table != NULL)
-	{
-		//if (m_table->isConnected())//TODO is connected on button?
-			return m_table->GetBoolean("pressed");
-		/*else
-			return false;*/
-	}
-	else
-		return false;
-}
-
-void Trigger::WhenActive(Command *command)
-{
-	PressedButtonScheduler *pbs = new PressedButtonScheduler(Grab(), this, command);
-	pbs->Start();
-}
-
-void Trigger::WhileActive(Command *command)
-{
-	HeldButtonScheduler *hbs = new HeldButtonScheduler(Grab(), this, command);
-	hbs->Start();
-}
-
-void Trigger::WhenInactive(Command *command)
-{
-	ReleasedButtonScheduler *rbs = new ReleasedButtonScheduler(Grab(), this, command);
-	rbs->Start();
-}
-
-void Trigger::CancelWhenActive(Command *command) {
-	CancelButtonScheduler *cbs = new CancelButtonScheduler(Grab(), this, command);
-	cbs->Start();
-}
-
-void Trigger::ToggleWhenActive(Command *command) {
-	ToggleButtonScheduler *tbs = new ToggleButtonScheduler(Grab(), this, command);
-	tbs->Start();
-}
-
-std::string Trigger::GetSmartDashboardType(){
-	return "Button";
-}
-
-void Trigger::InitTable(ITable* table){
-	m_table = table;
-	if(m_table!=NULL){
-		m_table->PutBoolean("pressed", Get());
-	}
-}
-
-ITable* Trigger::GetTable(){
-	return m_table;
-}

wpilibc/wpilibC++/lib/Commands/CommandGroup.cpp

@@ -1,383 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/CommandGroup.h"
-#include "WPIErrors.h"
-
-/**
- * Creates a new {@link CommandGroup CommandGroup}.
- */
-CommandGroup::CommandGroup()
-{
-	m_currentCommandIndex = -1;
-}
-
-/**
- * Creates a new {@link CommandGroup CommandGroup} with the given name.
- * @param name the name for this command group
- */
-CommandGroup::CommandGroup(const char *name) :
-	Command(name)
-{
-	m_currentCommandIndex = -1;
-}
-
-CommandGroup::~CommandGroup()
-{
-}
-
-/**
- * Adds a new {@link Command Command} to the group.  The {@link Command Command} will be started after
- * all the previously added {@link Command Commands}.
- *
- * <p>Note that any requirements the given {@link Command Command} has will be added to the
- * group.  For this reason, a {@link Command Command's} requirements can not be changed after
- * being added to a group.</p>
- *
- * <p>It is recommended that this method be called in the constructor.</p>
- * 
- * @param command The {@link Command Command} to be added
- */
-void CommandGroup::AddSequential(Command *command)
-{
-	if (command == NULL)
-	{
-		wpi_setWPIErrorWithContext(NullParameter, "command");
-		return;
-	}
-	if (!AssertUnlocked("Cannot add new command to command group"))
-		return;
-
-	command->SetParent(this);
-
-	m_commands.push_back(CommandGroupEntry(command, CommandGroupEntry::kSequence_InSequence));
-	// Iterate through command->GetRequirements() and call Requires() on each required subsystem
-	Command::SubsystemSet requirements = command->GetRequirements();
-	Command::SubsystemSet::iterator iter = requirements.begin();
-	for (; iter != requirements.end(); iter++)
-		Requires(*iter);
-}
-
-/**
- * Adds a new {@link Command Command} to the group with a given timeout.
- * The {@link Command Command} will be started after all the previously added commands.
- *
- * <p>Once the {@link Command Command} is started, it will be run until it finishes or the time
- * expires, whichever is sooner.  Note that the given {@link Command Command} will have no
- * knowledge that it is on a timer.</p>
- *
- * <p>Note that any requirements the given {@link Command Command} has will be added to the
- * group.  For this reason, a {@link Command Command's} requirements can not be changed after
- * being added to a group.</p>
- *
- * <p>It is recommended that this method be called in the constructor.</p>
- *
- * @param command The {@link Command Command} to be added
- * @param timeout The timeout (in seconds)
- */
-void CommandGroup::AddSequential(Command *command, double timeout)
-{
-	if (command == NULL)
-	{
-		wpi_setWPIErrorWithContext(NullParameter, "command");
-		return;
-	}
-	if (!AssertUnlocked("Cannot add new command to command group"))
-		return;
-	if (timeout < 0.0)
-	{
-		wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-		return;
-	}
-
-	command->SetParent(this);
-
-	m_commands.push_back(CommandGroupEntry(command, CommandGroupEntry::kSequence_InSequence, timeout));
-	// Iterate through command->GetRequirements() and call Requires() on each required subsystem
-	Command::SubsystemSet requirements = command->GetRequirements();
-	Command::SubsystemSet::iterator iter = requirements.begin();
-	for (; iter != requirements.end(); iter++)
-		Requires(*iter);
-}
-
-/**
- * Adds a new child {@link Command} to the group.  The {@link Command} will be started after
- * all the previously added {@link Command Commands}.
- *
- * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it
- * run at the same time as the subsequent {@link Command Commands}.  The child will run until either
- * it finishes, a new child with conflicting requirements is started, or
- * the main sequence runs a {@link Command} with conflicting requirements.  In the latter
- * two cases, the child will be canceled even if it says it can't be
- * interrupted.</p>
- *
- * <p>Note that any requirements the given {@link Command Command} has will be added to the
- * group.  For this reason, a {@link Command Command's} requirements can not be changed after
- * being added to a group.</p>
- *
- * <p>It is recommended that this method be called in the constructor.</p>
- *
- * @param command The command to be added
- */
-void CommandGroup::AddParallel(Command *command)
-{
-	if (command == NULL)
-	{
-		wpi_setWPIErrorWithContext(NullParameter, "command");
-		return;
-	}
-	if (!AssertUnlocked("Cannot add new command to command group"))
-		return;
-
-	command->SetParent(this);
-
-	m_commands.push_back(CommandGroupEntry(command, CommandGroupEntry::kSequence_BranchChild));
-	// Iterate through command->GetRequirements() and call Requires() on each required subsystem
-	Command::SubsystemSet requirements = command->GetRequirements();
-	Command::SubsystemSet::iterator iter = requirements.begin();
-	for (; iter != requirements.end(); iter++)
-		Requires(*iter);
-}
-
-/**
- * Adds a new child {@link Command} to the group with the given timeout.  The {@link Command} will be started after
- * all the previously added {@link Command Commands}.
- *
- * <p>Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
- * or the time expires, whichever is sooner.  Note that the given {@link Command Command} will have no
- * knowledge that it is on a timer.</p>
- *
- * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it
- * run at the same time as the subsequent {@link Command Commands}.  The child will run until either
- * it finishes, the timeout expires, a new child with conflicting requirements is started, or
- * the main sequence runs a {@link Command} with conflicting requirements.  In the latter
- * two cases, the child will be canceled even if it says it can't be
- * interrupted.</p>
- *
- * <p>Note that any requirements the given {@link Command Command} has will be added to the
- * group.  For this reason, a {@link Command Command's} requirements can not be changed after
- * being added to a group.</p>
- *
- * <p>It is recommended that this method be called in the constructor.</p>
- *
- * @param command The command to be added
- * @param timeout The timeout (in seconds)
- */
-void CommandGroup::AddParallel(Command *command, double timeout)
-{
-	if (command == NULL)
-	{
-		wpi_setWPIErrorWithContext(NullParameter, "command");
-		return;
-	}
-	if (!AssertUnlocked("Cannot add new command to command group"))
-		return;
-	if (timeout < 0.0)
-	{
-		wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-		return;
-	}
-
-	command->SetParent(this);
-
-	m_commands.push_back(CommandGroupEntry(command, CommandGroupEntry::kSequence_BranchChild, timeout));
-	// Iterate through command->GetRequirements() and call Requires() on each required subsystem
-	Command::SubsystemSet requirements = command->GetRequirements();
-	Command::SubsystemSet::iterator iter = requirements.begin();
-	for (; iter != requirements.end(); iter++)
-		Requires(*iter);
-}
-
-void CommandGroup::_Initialize()
-{
-	m_currentCommandIndex = -1;
-}
-
-void CommandGroup::_Execute()
-{
-	CommandGroupEntry entry;
-	Command *cmd = NULL;
-	bool firstRun = false;
-
-	if (m_currentCommandIndex == -1)
-	{
-		firstRun = true;
-		m_currentCommandIndex = 0;
-	}
-
-	while ((unsigned)m_currentCommandIndex < m_commands.size())
-	{
-		if (cmd != NULL)
-		{
-			if (entry.IsTimedOut())
-				cmd->_Cancel();
-
-			if (cmd->Run())
-			{
-				break;
-			}
-			else
-			{
-				cmd->Removed();
-				m_currentCommandIndex++;
-				firstRun = true;
-				cmd = NULL;
-				continue;
-			}
-		}
-
-		entry = m_commands[m_currentCommandIndex];
-		cmd = NULL;
-
-		switch (entry.m_state)
-		{
-			case CommandGroupEntry::kSequence_InSequence:
-				cmd = entry.m_command;
-				if (firstRun)
-				{
-					cmd->StartRunning();
-					CancelConflicts(cmd);
-					firstRun = false;
-				}
-				break;
-
-			case CommandGroupEntry::kSequence_BranchPeer:
-				m_currentCommandIndex++;
-				entry.m_command->Start();
-				break;
-
-			case CommandGroupEntry::kSequence_BranchChild:
-				m_currentCommandIndex++;
-				CancelConflicts(entry.m_command);
-				entry.m_command->StartRunning();
-				m_children.push_back(entry);
-				break;
-		}
-	}
-
-	// Run Children
-	CommandList::iterator iter = m_children.begin();
-	for (; iter != m_children.end();)
-	{
-		entry = *iter;
-		Command *child = entry.m_command;
-		if (entry.IsTimedOut())
-			child->_Cancel();
-
-		if (!child->Run())
-		{
-			child->Removed();
-			iter = m_children.erase(iter);
-		}
-		else
-		{
-			iter++;
-		}
-	}
-}
-
-void CommandGroup::_End()
-{
-	// Theoretically, we don't have to check this, but we do if teams override the IsFinished method
-	if (m_currentCommandIndex != -1 && (unsigned)m_currentCommandIndex < m_commands.size())
-	{
-		Command *cmd = m_commands[m_currentCommandIndex].m_command;
-		cmd->_Cancel();
-		cmd->Removed();
-	}
-
-	CommandList::iterator iter = m_children.begin();
-	for (; iter != m_children.end(); iter++)
-	{
-		Command *cmd = iter->m_command;
-		cmd->_Cancel();
-		cmd->Removed();
-	}
-	m_children.clear();
-}
-
-void CommandGroup::_Interrupted()
-{
-	_End();
-}
-
-// Can be overwritten by teams
-void CommandGroup::Initialize()
-{
-}
-
-// Can be overwritten by teams
-void CommandGroup::Execute()
-{
-}
-
-// Can be overwritten by teams
-void CommandGroup::End()
-{
-}
-
-// Can be overwritten by teams
-void CommandGroup::Interrupted()
-{
-}
-
-bool CommandGroup::IsFinished()
-{
-	return (unsigned)m_currentCommandIndex >= m_commands.size() && m_children.empty();
-}
-
-bool CommandGroup::IsInterruptible()
-{
-	if (!Command::IsInterruptible())
-		return false;
-
-	if (m_currentCommandIndex != -1 && (unsigned)m_currentCommandIndex < m_commands.size())
-	{
-		Command *cmd = m_commands[m_currentCommandIndex].m_command;
-		if (!cmd->IsInterruptible())
-			return false;
-	}
-
-	CommandList::iterator iter = m_children.begin();
-	for (; iter != m_children.end(); iter++)
-	{
-		if (!iter->m_command->IsInterruptible())
-			return false;
-	}
-
-	return true;
-}
-
-void CommandGroup::CancelConflicts(Command *command)
-{
-	CommandList::iterator childIter = m_children.begin();
-	for (; childIter != m_children.end();)
-	{
-		Command *child = childIter->m_command;
-		bool erased = false;
-
-		Command::SubsystemSet requirements = command->GetRequirements();
-		Command::SubsystemSet::iterator requirementIter = requirements.begin();
-		for (; requirementIter != requirements.end(); requirementIter++)
-		{
-			if (child->DoesRequire(*requirementIter))
-			{
-				child->_Cancel();
-				child->Removed();
-				childIter = m_children.erase(childIter);
-				erased = true;
-				break;
-			}
-		}
-		if (!erased)
-			childIter++;
-	}
-}
-
-int CommandGroup::GetSize()
-{
-	return m_children.size();
-}

wpilibc/wpilibC++/lib/Commands/CommandGroupEntry.cpp

@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/CommandGroupEntry.h"
-
-#include "Commands/Command.h"
-
-CommandGroupEntry::CommandGroupEntry() :
-	m_timeout(-1.0),
-	m_command(NULL),
-	m_state(kSequence_InSequence)
-{
-}
-
-CommandGroupEntry::CommandGroupEntry(Command *command, Sequence state) :
-	m_timeout(-1.0),
-	m_command(command),
-	m_state(state)
-{
-}
-
-CommandGroupEntry::CommandGroupEntry(Command *command, Sequence state, double timeout) :
-	m_timeout(timeout),
-	m_command(command),
-	m_state(state)
-{
-}
-
-bool CommandGroupEntry::IsTimedOut()
-{
-	if (m_timeout < 0.0)
-		return false;
-	double time = m_command->TimeSinceInitialized();
-	if (time == 0.0)
-		return false;
-	return time >= m_timeout;
-}

wpilibc/wpilibC++/lib/Commands/PIDCommand.cpp

@@ -1,106 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/PIDCommand.h"
-
-#include "PIDController.h"
-#include "float.h"
-
-PIDCommand::PIDCommand(const char *name, double p, double i, double d, double f, double period) :
-		Command(name)
-{
-	m_controller = new PIDController(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double f, double period)
-{
-	m_controller = new PIDController(p, i, d, f, this, this, period);
-}
-
-PIDCommand::PIDCommand(const char *name, double p, double i, double d) :
-	Command(name)
-{
-	m_controller = new PIDController(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(const char *name, double p, double i, double d, double period) :
-	Command(name)
-{
-	m_controller = new PIDController(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d)
-{
-	m_controller = new PIDController(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double period)
-{
-	m_controller = new PIDController(p, i, d, this, this, period);
-}
-
-PIDCommand::~PIDCommand()
-{
-	delete m_controller;
-}
-
-void PIDCommand::_Initialize()
-{
-	m_controller->Enable();
-}
-
-void PIDCommand::_End()
-{
-	m_controller->Disable();
-}
-
-void PIDCommand::_Interrupted()
-{
-	_End();
-}
-
-void PIDCommand::SetSetpointRelative(double deltaSetpoint)
-{
-	SetSetpoint(GetSetpoint() + deltaSetpoint);
-}
-
-void PIDCommand::PIDWrite(float output)
-{
-	UsePIDOutput(output);
-}
-
-double PIDCommand::PIDGet()
-{
-	return ReturnPIDInput();
-}
-
-PIDController *PIDCommand::GetPIDController()
-{
-	return m_controller;
-}
-
-void PIDCommand::SetSetpoint(double setpoint)
-{
-	m_controller->SetSetpoint(setpoint);
-}
-
-double PIDCommand::GetSetpoint()
-{
-	return m_controller->GetSetpoint();
-}
-
-double PIDCommand::GetPosition()
-{
-	return ReturnPIDInput();
-}
-
-std::string PIDCommand::GetSmartDashboardType(){
-        return "PIDCommand";
-}
-void PIDCommand::InitTable(ITable* table){
-	m_controller->InitTable(table);
-	Command::InitTable(table);
-}

wpilibc/wpilibC++/lib/Commands/PrintCommand.cpp

@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/PrintCommand.h"
-#include "stdio.h"
-#include <sstream>
-
-PrintCommand::PrintCommand(const char *message) :
-	Command(((std::stringstream&)(std::stringstream("Print \"") << message << "\"")).str().c_str())
-{
-	m_message = message;
-}
-
-void PrintCommand::Initialize()
-{
-	printf("%s", m_message.c_str());
-}
-
-void PrintCommand::Execute()
-{
-}
-
-bool PrintCommand::IsFinished()
-{
-	return true;
-}
-
-void PrintCommand::End()
-{
-}
-
-void PrintCommand::Interrupted()
-{
-}

wpilibc/wpilibC++/lib/Commands/Scheduler.cpp

@@ -1,285 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/Scheduler.h"
-
-#include "Buttons/ButtonScheduler.h"
-#include "Commands/Subsystem.h"
-//#include "NetworkCommunication/UsageReporting.h"
-#include "HAL/cpp/Synchronized.hpp"
-#include "WPIErrors.h"
-#include <iostream>
-#include <set>
-#include <algorithm>
-
-Scheduler *Scheduler::_instance = NULL;
-
-Scheduler::Scheduler() :
-	m_buttonsLock(NULL), m_additionsLock(NULL), m_adding(false) {
-	m_buttonsLock = initializeMutexNormal();
-	m_additionsLock = initializeMutexNormal();
-
-	HALReport(HALUsageReporting::kResourceType_Command,
-			HALUsageReporting::kCommand_Scheduler);
-
-	m_table = NULL;
-	m_enabled = true;
-}
-
-Scheduler::~Scheduler() {
-	takeMutex(m_additionsLock);
-	deleteMutex(m_additionsLock);
-
-	takeMutex(m_buttonsLock);
-	deleteMutex(m_buttonsLock);
-}
-
-/**
- * Returns the {@link Scheduler}, creating it if one does not exist.
- * @return the {@link Scheduler}
- */
-Scheduler *Scheduler::GetInstance() {
-	if (_instance == NULL)
-		_instance = new Scheduler();
-	return _instance;
-}
-
-void Scheduler::SetEnabled(bool enabled) {
-	m_enabled = enabled;
-}
-
-/**
- * Add a command to be scheduled later.
- * In any pass through the scheduler, all commands are added to the additions list, then
- * at the end of the pass, they are all scheduled.
- * @param command The command to be scheduled
- */
-void Scheduler::AddCommand(Command *command) {
-	Synchronized sync(m_additionsLock);
-	if (std::find(m_additions.begin(), m_additions.end(), command)
-			!= m_additions.end())
-		return;
-	m_additions.push_back(command);
-}
-
-void Scheduler::AddButton(ButtonScheduler *button) {
-	Synchronized sync(m_buttonsLock);
-	m_buttons.push_back(button);
-}
-
-void Scheduler::ProcessCommandAddition(Command *command) {
-	if (command == NULL)
-		return;
-
-	// Check to make sure no adding during adding
-	if (m_adding) {
-		wpi_setWPIErrorWithContext(IncompatibleState, "Can not start command from cancel method");
-		return;
-	}
-
-	// Only add if not already in
-	CommandSet::iterator found = m_commands.find(command);
-	if (found == m_commands.end()) {
-		// Check that the requirements can be had
-		Command::SubsystemSet requirements = command->GetRequirements();
-		Command::SubsystemSet::iterator iter;
-		for (iter = requirements.begin(); iter != requirements.end(); iter++) {
-			Subsystem *lock = *iter;
-			if (lock->GetCurrentCommand() != NULL
-					&& !lock->GetCurrentCommand()->IsInterruptible())
-				return;
-		}
-
-		// Give it the requirements
-		m_adding = true;
-		for (iter = requirements.begin(); iter != requirements.end(); iter++) {
-			Subsystem *lock = *iter;
-			if (lock->GetCurrentCommand() != NULL) {
-				lock->GetCurrentCommand()->Cancel();
-				Remove(lock->GetCurrentCommand());
-			}
-			lock->SetCurrentCommand(command);
-		}
-		m_adding = false;
-
-		m_commands.insert(command);
-
-		command->StartRunning();
-		m_runningCommandsChanged = true;
-	}
-}
-
-/**
- * Runs a single iteration of the loop.  This method should be called often in order to have a functioning
- * {@link Command} system.  The loop has five stages:
- *
- * <ol>
- * <li> Poll the Buttons </li>
- * <li> Execute/Remove the Commands </li>
- * <li> Send values to SmartDashboard </li>
- * <li> Add Commands </li>
- * <li> Add Defaults </li>
- * </ol>
- */
-void Scheduler::Run() {
-	// Get button input (going backwards preserves button priority)
-	{
-		if (!m_enabled)
-			return;
-
-		Synchronized sync(m_buttonsLock);
-		ButtonVector::reverse_iterator rButtonIter = m_buttons.rbegin();
-		for (; rButtonIter != m_buttons.rend(); rButtonIter++) {
-			(*rButtonIter)->Execute();
-		}
-	}
-	
-	m_runningCommandsChanged = false;
-
-	// Loop through the commands
-	CommandSet::iterator commandIter = m_commands.begin();
-	for (; commandIter != m_commands.end();) {
-		Command *command = *commandIter;
-		// Increment before potentially removing to keep the iterator valid
-		commandIter++;
-		if (!command->Run()) {
-			Remove(command);
-			m_runningCommandsChanged = true;
-		}
-	}
-
-	// Add the new things
-	{
-		Synchronized sync(m_additionsLock);
-		CommandVector::iterator additionsIter = m_additions.begin();
-		for (; additionsIter != m_additions.end(); additionsIter++) {
-			ProcessCommandAddition(*additionsIter);
-		}
-		m_additions.clear();
-	}
-
-	// Add in the defaults
-	Command::SubsystemSet::iterator subsystemIter = m_subsystems.begin();
-	for (; subsystemIter != m_subsystems.end(); subsystemIter++) {
-		Subsystem *lock = *subsystemIter;
-		if (lock->GetCurrentCommand() == NULL) {
-			ProcessCommandAddition(lock->GetDefaultCommand());
-		}
-		lock->ConfirmCommand();
-	}
-
-	UpdateTable();
-}
-
-/**
- * Registers a {@link Subsystem} to this {@link Scheduler}, so that the {@link Scheduler} might know
- * if a default {@link Command} needs to be run.  All {@link Subsystem Subsystems} should call this.
- * @param system the system
- */
-void Scheduler::RegisterSubsystem(Subsystem *subsystem) {
-	if (subsystem == NULL) {
-		wpi_setWPIErrorWithContext(NullParameter, "subsystem");
-		return;
-	}
-	m_subsystems.insert(subsystem);
-}
-
-/**
- * Removes the {@link Command} from the {@link Scheduler}.
- * @param command the command to remove
- */
-void Scheduler::Remove(Command *command) {
-	if (command == NULL) {
-		wpi_setWPIErrorWithContext(NullParameter, "command");
-		return;
-	}
-
-	if (!m_commands.erase(command))
-		return;
-
-	Command::SubsystemSet requirements = command->GetRequirements();
-	Command::SubsystemSet::iterator iter = requirements.begin();
-	for (; iter != requirements.end(); iter++) {
-		Subsystem *lock = *iter;
-		lock->SetCurrentCommand(NULL);
-	}
-
-	command->Removed();
-}
-
-void Scheduler::RemoveAll() {
-	while (m_commands.size() > 0) {
-		Remove(*m_commands.begin());
-	}
-}
-
-/**
- * Update the network tables associated with the Scheduler object on the SmartDashboard
- */
-void Scheduler::UpdateTable() {
-	CommandSet::iterator commandIter;
-	if (m_table != NULL) {
-		// Get the list of possible commands to cancel
-		m_table->RetrieveValue("Cancel", *toCancel);
-//		m_table->RetrieveValue("Ids", *ids);
-
-		// cancel commands that have had the cancel buttons pressed
-		// on the SmartDashboad
-		if (toCancel->size() > 0) {
-			for (commandIter = m_commands.begin(); commandIter
-					!= m_commands.end(); ++commandIter) {
-				for (unsigned i = 0; i < toCancel->size(); i++) {
-					Command *c = *commandIter;
-					if (c->GetID() == toCancel->get(i)) {
-						c->Cancel();
-					}
-				}
-			}
-			toCancel->setSize(0);
-			m_table->PutValue("Cancel", *toCancel);
-		}
-		
-		// Set the running commands
-		if (m_runningCommandsChanged) {
-			commands->setSize(0);
-			ids->setSize(0);
-			for (commandIter = m_commands.begin(); commandIter != m_commands.end(); ++commandIter) {
-				Command *c = *commandIter;
-				commands->add(c->GetName());
-				ids->add(c->GetID());
-			}
-			m_table->PutValue("Names", *commands);
-			m_table->PutValue("Ids", *ids);
-		}
-	}
-}
-
-std::string Scheduler::GetName() {
-	return "Scheduler";
-}
-
-std::string Scheduler::GetType() {
-	return "Scheduler";
-}
-
-std::string Scheduler::GetSmartDashboardType() {
-	return "Scheduler";
-}
-
-void Scheduler::InitTable(ITable *subTable) {
-	m_table = subTable;
-	commands = new StringArray();
-	ids = new NumberArray();
-	toCancel = new NumberArray();
-
-	m_table->PutValue("Names", *commands);
-	m_table->PutValue("Ids", *ids);
-	m_table->PutValue("Cancel", *toCancel);
-}
-
-ITable * Scheduler::GetTable() {
-	return m_table;
-}

wpilibc/wpilibC++/lib/Commands/StartCommand.cpp

@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/StartCommand.h"
-
-StartCommand::StartCommand(Command *commandToStart) :
-	Command("StartCommand")
-{
-	m_commandToFork = commandToStart;
-}
-
-void StartCommand::Initialize()
-{
-	m_commandToFork->Start();
-}
-
-void StartCommand::Execute()
-{
-}
-
-void StartCommand::End()
-{
-}
-
-void StartCommand::Interrupted()
-{
-}
-
-bool StartCommand::IsFinished()
-{
-    return true;
-}

wpilibc/wpilibC++/lib/Commands/Subsystem.cpp

@@ -1,179 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/Subsystem.h"
-
-#include "Commands/Command.h"
-#include "Commands/Scheduler.h"
-#include "WPIErrors.h"
-
-/**
- * Creates a subsystem with the given name
- * @param name the name of the subsystem
- */
-Subsystem::Subsystem(const char *name) :
-	m_currentCommand(NULL),
-	m_defaultCommand(NULL),
-	m_initializedDefaultCommand(false)
-{
-	m_name = name;
-	Scheduler::GetInstance()->RegisterSubsystem(this);
-	m_table = NULL;
-	m_currentCommandChanged = true;
-}
-/**
- * Initialize the default command for this subsystem
- * This is meant to be the place to call SetDefaultCommand in a subsystem and will be called
- * on all the subsystems by the CommandBase method before the program starts running by using
- * the list of all registered Subsystems inside the Scheduler.
- * 
- * This should be overridden by a Subsystem that has a default Command
- */
-void Subsystem::InitDefaultCommand() {
-
-}
-
-/**
- * Sets the default command.  If this is not called or is called with null,
- * then there will be no default command for the subsystem.
- *
- * <p><b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
- * singleton.</p>
- *
- * @param command the default command (or null if there should be none)
- */
-void Subsystem::SetDefaultCommand(Command *command)
-{
-	if (command == NULL)
-	{
-		m_defaultCommand = NULL;
-	}
-	else
-	{
-		bool found = false;
-		Command::SubsystemSet requirements = command->GetRequirements();
-		Command::SubsystemSet::iterator iter = requirements.begin();
-		for (; iter != requirements.end(); iter++)
-		{
-			if (*iter == this)
-			{
-				found = true;
-				break;
-			}
-		}
-
-		if (!found)
-		{
-			wpi_setWPIErrorWithContext(CommandIllegalUse, "A default command must require the subsystem");
-			return;
-		}
-		
-		m_defaultCommand = command;
-	}
-	if (m_table != NULL)
-	{
-		if (m_defaultCommand != NULL)
-		{
-			m_table->PutBoolean("hasDefault", true);
-			m_table->PutString("default", m_defaultCommand->GetName());
-		}
-		else
-		{
-			m_table->PutBoolean("hasDefault", false);
-		}
-	}
-}
-
-/**
- * Returns the default command (or null if there is none).
- * @return the default command
- */
-Command *Subsystem::GetDefaultCommand()
-{
-	if (!m_initializedDefaultCommand) {
-		m_initializedDefaultCommand = true;
-		InitDefaultCommand();
-	}
-	return m_defaultCommand;
-}
-
-/**
- * Sets the current command
- * @param command the new current command
- */
-void Subsystem::SetCurrentCommand(Command *command)
-{
-	m_currentCommand = command;
-	m_currentCommandChanged = true;
-}
-
-/**
- * Returns the command which currently claims this subsystem.
- * @return the command which currently claims this subsystem
- */
-Command *Subsystem::GetCurrentCommand()
-{
-	return m_currentCommand;
-}
-
-/**
- * Call this to alert Subsystem that the current command is actually the command.
- * Sometimes, the {@link Subsystem} is told that it has no command while the {@link Scheduler}
- * is going through the loop, only to be soon after given a new one.  This will avoid that situation.
- */
-void Subsystem::ConfirmCommand()
-{
-	if (m_currentCommandChanged) {
-		if (m_table != NULL)
-		{
-			if (m_currentCommand != NULL)
-			{
-				m_table->PutBoolean("hasCommand", true);
-				m_table->PutString("command", m_currentCommand->GetName());
-			}
-			else
-			{
-				m_table->PutBoolean("hasCommand", false);
-			}
-		}
-		m_currentCommandChanged = false;
-	}
-}
-
-
-
-std::string Subsystem::GetName()
-{
-	return m_name;
-}
-
-std::string Subsystem::GetSmartDashboardType()
-{
-	return "Subsystem";
-}
-
-void Subsystem::InitTable(ITable* table)
-{
-    m_table = table;
-    if(m_table!=NULL){
-        if (m_defaultCommand != NULL) {
-        	m_table->PutBoolean("hasDefault", true);
-        	m_table->PutString("default", m_defaultCommand->GetName());
-        } else {
-        	m_table->PutBoolean("hasDefault", false);
-        }
-        if (m_currentCommand != NULL) {
-        	m_table->PutBoolean("hasCommand", true);
-            m_table->PutString("command", m_currentCommand->GetName());
-        } else {
-        	m_table->PutBoolean("hasCommand", false);
-        }
-    }
-}
-
-ITable* Subsystem::GetTable(){
-	return m_table;
-}

wpilibc/wpilibC++/lib/Commands/WaitCommand.cpp

@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/WaitCommand.h"
-#include <sstream>
-
-WaitCommand::WaitCommand(double timeout) :
-	Command(((std::stringstream&)(std::stringstream("Wait(") << timeout << ")")).str().c_str(), timeout)
-{
-}
-
-WaitCommand::WaitCommand(const char *name, double timeout) :
-	Command(name, timeout)
-{
-}
-
-void WaitCommand::Initialize()
-{
-}
-
-void WaitCommand::Execute()
-{
-}
-
-bool WaitCommand::IsFinished()
-{
-    return IsTimedOut();
-}
-
-void WaitCommand::End()
-{
-}
-
-void WaitCommand::Interrupted()
-{
-}

wpilibc/wpilibC++/lib/Commands/WaitForChildren.cpp

@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "Commands/WaitForChildren.h"
-#include "Commands/CommandGroup.h"
-
-WaitForChildren::WaitForChildren(double timeout) :
-	Command("WaitForChildren", timeout)
-{	
-}
-
-WaitForChildren::WaitForChildren(const char *name, double timeout) :
-	Command(name, timeout)
-{
-}
-
-void WaitForChildren::Initialize()
-{
-}
-
-void WaitForChildren::Execute()
-{
-}
-
-void WaitForChildren::End()
-{
-}
-
-void WaitForChildren::Interrupted()
-{
-}
-
-bool WaitForChildren::IsFinished()
-{
-    return GetGroup() == NULL || GetGroup()->GetSize() == 0;
-}

wpilibc/wpilibC++/lib/LiveWindow/LiveWindowStatusListener.cpp

@@ -1,7 +0,0 @@
-#include "LiveWindow/LiveWindowStatusListener.h"
-#include "Commands/Scheduler.h"
-
-void LiveWindowStatusListener::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
-	
-}
-

wpilibc/wpilibC++/lib/SmartDashboard/SendableChooser.cpp

@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2011. 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 "SmartDashboard/SendableChooser.h"
-#include "networktables2/type/StringArray.h"
-
-#include <stdio.h>
-
-static const char *kDefault = "default";
-static const char *kOptions = "options";
-static const char *kSelected = "selected";
-
-SendableChooser::SendableChooser()
-{
-	m_defaultChoice = "";
-}
-
-/**
- * Adds the given object to the list of options.  On the {@link SmartDashboard} on the desktop,
- * the object will appear as the given name.
- * @param name the name of the option
- * @param object the option
- */
-void SendableChooser::AddObject(const char *name, void *object)
-{
-	m_choices[name] = object;
-}
-
-/**
- * Add the given object to the list of options and marks it as the default.
- * Functionally, this is very close to {@link SendableChooser#AddObject(const char *name, void *object) AddObject(...)}
- * except that it will use this as the default option if none other is explicitly selected.
- * @param name the name of the option
- * @param object the option
- */
-void SendableChooser::AddDefault(const char *name, void *object)
-{
-	m_defaultChoice = name;
-	AddObject(name, object);
-}
-
-/**
- * Returns the selected option.  If there is none selected, it will return the default.  If there is none selected
- * and no default, then it will return {@code NULL}.
- * @return the option selected
- */
-void *SendableChooser::GetSelected()
-{
-	std::string selected = m_table->GetString(kSelected, m_defaultChoice);
-	if (selected == "")
-		return NULL;
-	else
-		return m_choices[selected];
-}
-
-void SendableChooser::InitTable(ITable* subtable) {
-	StringArray keys;
-	m_table = subtable;
-	if (m_table != NULL) {
-		std::map<std::string, void *>::iterator iter;
-		for (iter = m_choices.begin(); iter != m_choices.end(); iter++) {
-			keys.add(iter->first);
-		}
-		m_table->PutValue(kOptions, keys);
-		m_table->PutString(kDefault, m_defaultChoice);
-	}
-}
-
-ITable* SendableChooser::GetTable() {
-	return m_table;
-}
-
-std::string SendableChooser::GetSmartDashboardType() {
-	return "String Chooser";
-}

wpilibc/wpilibC++/src/Commands/Command.cpp

@@ -7,7 +7,7 @@
 #include "Commands/Command.h"
 #include "Commands/CommandGroup.h"
 #include "Commands/Scheduler.h"
-#include "DriverStation.h"
+#include "RobotState.h"
 #include "Timer.h"
 #include "WPIErrors.h"
 
@@ -198,7 +198,7 @@ void Command::Start()
  */
 bool Command::Run()
 {
-	if (!m_runWhenDisabled && m_parent == NULL && DriverStation::GetInstance()->IsDisabled())
+    if (!m_runWhenDisabled && m_parent == NULL && RobotState::IsDisabled())
 		Cancel();
 
 	if (IsCanceled())

wpilibc/wpilibC++/src/Commands/Scheduler.cpp

@@ -8,6 +8,7 @@
 
 #include "Buttons/ButtonScheduler.h"
 #include "Commands/Subsystem.h"
+#include "HLUsageReporting.h"
 #include "HAL/cpp/Synchronized.hpp"
 #include "WPIErrors.h"
 #include <iostream>
@@ -21,6 +22,8 @@ Scheduler::Scheduler() :
 	m_buttonsLock = initializeMutexNormal();
 	m_additionsLock = initializeMutexNormal();
 
+    HLUsageReporting::ReportScheduler();
+
 	m_table = NULL;
 	m_enabled = true;
 }

wpilibc/wpilibC++/src/Commands/WaitUntilCommand.cpp

@@ -5,7 +5,6 @@
 /*----------------------------------------------------------------------------*/
 
 #include "Commands/WaitUntilCommand.h"
-#include "DriverStation.h"
 #include "Timer.h"
 
 /**
@@ -39,7 +38,7 @@ void WaitUntilCommand::Execute()
  */
 bool WaitUntilCommand::IsFinished()
 {
-    return DriverStation::GetInstance()->GetMatchTime() >= m_time;
+    return Timer::GetMatchTime() >= m_time;
 }
 
 void WaitUntilCommand::End()

wpilibc/wpilibC++/src/Error.cpp

@@ -11,6 +11,8 @@
 #include <cstdio>
 #include <cstring>
 #include <cstdlib>
+#include <stdint.h>
+#include "HAL/Task.hpp"
 
 //#include "NetworkCommunication/FRCComm.h"
 #include "Timer.h"
@@ -71,7 +73,7 @@ void Error::Set(Code code, const char* contextMessage, const char* filename, con
 
 	Report();
 
-	if (m_suspendOnErrorEnabled) suspendTask(0);
+    if (m_suspendOnErrorEnabled) suspendTask(0);
 }
 
 void Error::Report()
@@ -87,7 +89,7 @@ void Error::Report()
 
 	// Print the error and send it to the DriverStation
 	std::cout << error << std::endl;
-	HALSetErrorData(error.c_str(), error.size(), 100);
+    // TODO: Better logging HALSetErrorData(error.c_str(), error.size(), 100);
 }
 
 void Error::Clear()

wpilibc/wpilibC++/src/ErrorBase.cpp

@@ -6,7 +6,6 @@
 
 #include "ErrorBase.h"
 #include "HAL/cpp/Synchronized.hpp"
-#include "nivision.h"
 #define WPI_ERRORS_DEFINE_STRINGS
 #include "WPIErrors.h"
 

wpilibc/wpilibC++/src/HLUsageReporting.cpp

@@ -0,0 +1,20 @@
+
+#include "HLUsageReporting.h"
+
+HLUsageReportingInterface* HLUsageReporting::impl = 0;
+
+void HLUsageReporting::SetImplementation(HLUsageReportingInterface* i) {
+    impl = i;
+}
+
+void HLUsageReporting::ReportScheduler() {
+    if (impl != 0) {
+        impl->ReportScheduler();
+    }
+}
+
+void HLUsageReporting::ReportSmartDashboard() {
+    if (impl != 0) {
+        impl->ReportSmartDashboard();
+    }
+}

wpilibc/wpilibC++/src/RobotState.cpp

@@ -0,0 +1,42 @@
+#include "RobotState.h"
+
+RobotStateInterface* RobotState::impl = 0;
+
+void RobotState::SetImplementation(RobotStateInterface* i) {
+    impl = i;
+}
+
+bool RobotState::IsDisabled() {
+    if (impl != 0) {
+        return impl->IsDisabled();
+    }
+    return true;
+}
+
+bool RobotState::IsEnabled() {
+    if (impl != 0) {
+        impl->IsEnabled();
+    }
+    return false;
+}
+
+bool RobotState::IsOperatorControl() {
+    if (impl != 0) {
+        impl->IsOperatorControl();
+    }
+    return true;
+}
+
+bool RobotState::IsAutonomous() {
+    if (impl != 0) {
+        impl->IsAutonomous();
+    }
+    return false;
+}
+
+bool RobotState::IsTest() {
+    if (impl != 0) {
+        impl->IsTest();
+    }
+    return false;
+}

wpilibc/wpilibC++/src/SmartDashboard/SmartDashboard.cpp

@@ -10,6 +10,7 @@
 #include "SmartDashboard/NamedSendable.h"
 #include "WPIErrors.h"
 #include "networktables/NetworkTable.h"
+#include "HLUsageReporting.h"
 
 ITable* SmartDashboard::m_table = NULL;
 std::map<ITable *, Sendable *> SmartDashboard::m_tablesToData;
@@ -17,7 +18,7 @@ std::map<ITable *, Sendable *> SmartDashboard::m_tablesToData;
 void SmartDashboard::init(){
 	m_table = NetworkTable::GetTable("SmartDashboard");
 
-	HALReport(HALUsageReporting::kResourceType_SmartDashboard, 0);
+    HLUsageReporting::ReportSmartDashboard();
 }
 
 /**

wpilibc/wpilibC++Devices/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8)
+project(WPILibC++Devices)
+
+file(GLOB_RECURSE SRC_FILES src/*.cpp)
+include_directories(include/ ${WPILIB_INCLUDES} ${HAL_API_INCLUDES} ${NWT_API_INCLUDES})
+add_library(WPILibAthena STATIC ${SRC_FILES})
+target_link_libraries(WPILibAthena WPILib HALAthena NetworkTables ${NI_LIBS})
+INSTALL(TARGETS WPILibAthena ARCHIVE DESTINATION src COMPONENT src)
+INSTALL(DIRECTORY include DESTINATION ${CMAKE_INSTALL_PREFIX} COMPONENT headers)
+# lib/ c m gcc_s ld-linux
+# usr/lib stdc++
+# ni_emb
+# HAL NWT

wpilibc/wpilibC++Devices/include/DriverStation.h

@@ -6,6 +6,7 @@
 #pragma once
 
 #include "SensorBase.h"
+#include "RobotState.h"
 #include "Task.h"
 
 struct HALControlWord;
@@ -14,7 +15,7 @@ class AnalogInput;
 /**
  * Provide access to the network communication data to / from the Driver Station.
  */
-class DriverStation : public SensorBase
+class DriverStation : public SensorBase, public RobotStateInterface
 {
 public:
 	enum Alliance
@@ -90,7 +91,6 @@ protected:
 private:
 	static void InitTask(DriverStation *ds);
 	static DriverStation *m_instance;
-	static uint8_t m_updateNumber;
 
 	void Run();
 
@@ -99,7 +99,6 @@ private:
 	HALJoystickAxes m_joystickAxes[kJoystickPorts];
 	HALJoystickButtons m_joystickButtons[kJoystickPorts];
 
-	uint8_t m_digitalOut;
 	MUTEX_ID m_statusDataSemaphore;
 	Task m_task;
 	SEMAPHORE_ID m_newControlData;

wpilibc/wpilibC++Devices/include/Internal/HardwareHLReporting.h

@@ -0,0 +1,9 @@
+
+#include "HLUsageReporting.h"
+
+class HardwareHLReporting : public HLUsageReportingInterface
+{
+public:
+    virtual void ReportScheduler();
+    virtual void ReportSmartDashboard();
+};