This is the latest WPILib src, VisionSample2013, cRIO image, ... pulled down from firstforge.wpi.edu.
There might be risks in using the top of tree rather than an official release, but the commit messages do mention fixes for some deadlocks and race conditions.
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4066 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Command.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Command.cpp
new file mode 100644
index 0000000..aea6fdf
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Command.cpp
@@ -0,0 +1,463 @@
+/*----------------------------------------------------------------------------*/
+/* 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/Command.h"
+#include "Commands/CommandGroup.h"
+#include "Commands/Scheduler.h"
+#include "DriverStation.h"
+#include "Timer.h"
+#include "WPIErrors.h"
+
+static const char *kName = "name";
+static const char *kRunning = "running";
+static const char *kIsParented = "isParented";
+
+void Command::InitCommand(const char *name, double timeout)
+{
+ m_timeout = timeout;
+ m_locked = false;
+ m_startTime = -1;
+ m_initialized = false;
+ m_running = false;
+ m_interruptible = true;
+ m_canceled = false;
+ m_runWhenDisabled = false;
+ m_parent = NULL;
+ if (name == NULL)
+ {
+ // Don't have a way to find the subclass name like java, so use the address
+ char buf[32];
+ snprintf(buf, 32, "Command_%p", this);
+ m_name = buf;
+ }
+ else
+ {
+ m_name = name;
+ }
+ m_table = NULL;
+}
+
+/**
+ * Creates a new command.
+ * The name of this command will be default.
+ */
+Command::Command()
+{
+ InitCommand(NULL, -1.0);
+}
+
+/**
+ * Creates a new command with the given name and no timeout.
+ * @param name the name for this command
+ */
+Command::Command(const char *name)
+{
+ if (name == NULL)
+ wpi_setWPIErrorWithContext(NullParameter, "name");
+ InitCommand(name, -1.0);
+}
+
+/**
+ * Creates a new command with the given timeout and a default name.
+ * @param timeout the time (in seconds) before this command "times out"
+ * @see Command#isTimedOut() isTimedOut()
+ */
+Command::Command(double timeout)
+{
+ if (timeout < 0.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ InitCommand(NULL, timeout);
+}
+
+/**
+ * Creates a new command with the given name and timeout.
+ * @param name the name of the command
+ * @param timeout the time (in seconds) before this command "times out"
+ * @see Command#isTimedOut() isTimedOut()
+ */
+Command::Command(const char *name, double timeout)
+{
+ if (name == NULL)
+ wpi_setWPIErrorWithContext(NullParameter, "name");
+ if (timeout < 0.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ InitCommand(name, timeout);
+}
+
+Command::~Command()
+{//TODO deal with cleaning up all listeners
+ /*if (m_table != NULL){
+ m_table->RemoveChangeListener(kRunning, this);
+ }*/
+}
+
+/**
+ * Sets the timeout of this command.
+ * @param timeout the timeout (in seconds)
+ * @see Command#isTimedOut() isTimedOut()
+ */
+void Command::SetTimeout(double timeout)
+{
+ if (timeout < 0.0)
+ wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+ else
+ m_timeout = timeout;
+}
+
+/**
+ * Returns the time since this command was initialized (in seconds).
+ * This function will work even if there is no specified timeout.
+ * @return the time since this command was initialized (in seconds).
+ */
+double Command::TimeSinceInitialized()
+{
+ if (m_startTime < 0.0)
+ return 0.0;
+ else
+ return Timer::GetFPGATimestamp() - m_startTime;
+}
+
+/**
+ * This method specifies that the given {@link Subsystem} is used by this command.
+ * This method is crucial to the functioning of the Command System in general.
+ *
+ * <p>Note that the recommended way to call this method is in the constructor.</p>
+ *
+ * @param subsystem the {@link Subsystem} required
+ * @see Subsystem
+ */
+void Command::Requires(Subsystem *subsystem)
+{
+ if (!AssertUnlocked("Can not add new requirement to command"))
+ return;
+
+ if (subsystem != NULL)
+ m_requirements.insert(subsystem);
+ else
+ wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+}
+
+/**
+ * Called when the command has been removed.
+ * This will call {@link Command#interrupted() interrupted()} or {@link Command#end() end()}.
+ */
+void Command::Removed()
+{
+ if (m_initialized)
+ {
+ if (IsCanceled())
+ {
+ Interrupted();
+ _Interrupted();
+ }
+ else
+ {
+ End();
+ _End();
+ }
+ }
+ m_initialized = false;
+ m_canceled = false;
+ m_running = false;
+ if (m_table != NULL)
+ m_table->PutBoolean(kRunning, false);
+}
+
+/**
+ * Starts up the command. Gets the command ready to start.
+ * <p>Note that the command will eventually start, however it will not necessarily
+ * do so immediately, and may in fact be canceled before initialize is even called.</p>
+ */
+void Command::Start()
+{
+ LockChanges();
+ if (m_parent != NULL)
+ wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not start a command that is part of a command group");
+
+ Scheduler::GetInstance()->AddCommand(this);
+}
+
+/**
+ * The run method is used internally to actually run the commands.
+ * @return whether or not the command should stay within the {@link Scheduler}.
+ */
+bool Command::Run()
+{
+ if (!m_runWhenDisabled && m_parent == NULL && DriverStation::GetInstance()->IsDisabled())
+ Cancel();
+
+ if (IsCanceled())
+ return false;
+
+ if (!m_initialized)
+ {
+ m_initialized = true;
+ StartTiming();
+ _Initialize();
+ Initialize();
+ }
+ _Execute();
+ Execute();
+ return !IsFinished();
+}
+
+void Command::_Initialize()
+{
+}
+
+void Command::_Interrupted()
+{
+}
+
+void Command::_Execute()
+{
+}
+
+void Command::_End()
+{
+}
+
+/**
+ * Called to indicate that the timer should start.
+ * This is called right before {@link Command#initialize() initialize()} is, inside the
+ * {@link Command#run() run()} method.
+ */
+void Command::StartTiming()
+{
+ m_startTime = Timer::GetFPGATimestamp();
+}
+
+/**
+ * Returns whether or not the {@link Command#timeSinceInitialized() timeSinceInitialized()}
+ * method returns a number which is greater than or equal to the timeout for the command.
+ * If there is no timeout, this will always return false.
+ * @return whether the time has expired
+ */
+bool Command::IsTimedOut()
+{
+ return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
+}
+
+/**
+ * Returns the requirements (as an std::set of {@link Subsystem Subsystems} pointers) of this command
+ * @return the requirements (as an std::set of {@link Subsystem Subsystems} pointers) of this command
+ */
+Command::SubsystemSet Command::GetRequirements()
+{
+ return m_requirements;
+}
+
+/**
+ * Prevents further changes from being made
+ */
+void Command::LockChanges()
+{
+ m_locked = true;
+}
+
+/**
+ * If changes are locked, then this will generate a CommandIllegalUse error.
+ * @param message the message to report on error (it is appended by a default message)
+ * @return true if assert passed, false if assert failed
+ */
+bool Command::AssertUnlocked(const char *message)
+{
+ if (m_locked)
+ {
+ char buf[128];
+ snprintf(buf, 128, "%s after being started or being added to a command group", message);
+ wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
+ return false;
+ }
+ return true;
+}
+
+/**
+ * Sets the parent of this command. No actual change is made to the group.
+ * @param parent the parent
+ */
+void Command::SetParent(CommandGroup *parent)
+{
+ if (parent == NULL)
+ {
+ wpi_setWPIErrorWithContext(NullParameter, "parent");
+ }
+ else if (m_parent != NULL)
+ {
+ wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not give command to a command group after already being put in a command group");
+ }
+ else
+ {
+ LockChanges();
+ m_parent = parent;
+ if (m_table != NULL)
+ {
+ m_table->PutBoolean(kIsParented, true);
+ }
+ }
+}
+
+/**
+ * This is used internally to mark that the command has been started.
+ * The lifecycle of a command is:
+ *
+ * startRunning() is called.
+ * run() is called (multiple times potentially)
+ * removed() is called
+ *
+ * It is very important that startRunning and removed be called in order or some assumptions
+ * of the code will be broken.
+ */
+void Command::StartRunning()
+{
+ m_running = true;
+ m_startTime = -1;
+ if (m_table != NULL)
+ m_table->PutBoolean(kRunning, true);
+}
+
+/**
+ * Returns whether or not the command is running.
+ * This may return true even if the command has just been canceled, as it may
+ * not have yet called {@link Command#interrupted()}.
+ * @return whether or not the command is running
+ */
+bool Command::IsRunning()
+{
+ return m_running;
+}
+
+/**
+ * This will cancel the current command.
+ * <p>This will cancel the current command eventually. It can be called multiple times.
+ * And it can be called when the command is not running. If the command is running though,
+ * then the command will be marked as canceled and eventually removed.</p>
+ * <p>A command can not be canceled
+ * if it is a part of a command group, you must cancel the command group instead.</p>
+ */
+void Command::Cancel()
+{
+ if (m_parent != NULL)
+ wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not cancel a command that is part of a command group");
+
+ _Cancel();
+}
+
+/**
+ * This works like cancel(), except that it doesn't throw an exception if it is a part
+ * of a command group. Should only be called by the parent command group.
+ */
+void Command::_Cancel()
+{
+ if (IsRunning())
+ m_canceled = true;
+}
+
+/**
+ * Returns whether or not this has been canceled.
+ * @return whether or not this has been canceled
+ */
+bool Command::IsCanceled()
+{
+ return m_canceled;
+}
+
+/**
+ * Returns whether or not this command can be interrupted.
+ * @return whether or not this command can be interrupted
+ */
+bool Command::IsInterruptible()
+{
+ return m_interruptible;
+}
+
+/**
+ * Sets whether or not this command can be interrupted.
+ * @param interruptible whether or not this command can be interrupted
+ */
+void Command::SetInterruptible(bool interruptible)
+{
+ m_interruptible = interruptible;
+}
+
+/**
+ * Checks if the command requires the given {@link Subsystem}.
+ * @param system the system
+ * @return whether or not the subsystem is required (false if given NULL)
+ */
+bool Command::DoesRequire(Subsystem *system)
+{
+ return m_requirements.count(system) > 0;
+}
+
+/**
+ * Returns the {@link CommandGroup} that this command is a part of.
+ * Will return null if this {@link Command} is not in a group.
+ * @return the {@link CommandGroup} that this command is a part of (or null if not in group)
+ */
+CommandGroup *Command::GetGroup()
+{
+ return m_parent;
+}
+
+/**
+ * Sets whether or not this {@link Command} should run when the robot is disabled.
+ *
+ * <p>By default a command will not run when the robot is disabled, and will in fact be canceled.</p>
+ * @param run whether or not this command should run when the robot is disabled
+ */
+void Command::SetRunWhenDisabled(bool run)
+{
+ m_runWhenDisabled = run;
+}
+
+/**
+ * Returns whether or not this {@link Command} will run when the robot is disabled, or if it will cancel itself.
+ * @return whether or not this {@link Command} will run when the robot is disabled, or if it will cancel itself
+ */
+bool Command::WillRunWhenDisabled()
+{
+ return m_runWhenDisabled;
+}
+
+std::string Command::GetName()
+{
+ return m_name;
+}
+
+std::string Command::GetSmartDashboardType()
+{
+ return "Command";
+}
+
+void Command::InitTable(ITable* table)
+{
+ if(m_table!=NULL)
+ m_table->RemoveTableListener(this);
+ m_table = table;
+ if(m_table!=NULL){
+ m_table->PutString(kName, GetName());
+ m_table->PutBoolean(kRunning, IsRunning());
+ m_table->PutBoolean(kIsParented, m_parent != NULL);
+ m_table->AddTableListener(kRunning, this, false);
+ }
+}
+
+ITable* Command::GetTable(){
+ return m_table;
+}
+
+void Command::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew)
+{
+ if (value.b){
+ if(!IsRunning())
+ Start();
+ }
+ else{
+ if(IsRunning())
+ Cancel();
+ }
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Command.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Command.h
new file mode 100644
index 0000000..760c8f5
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Command.h
@@ -0,0 +1,162 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __COMMAND_H__
+#define __COMMAND_H__
+
+#include "ErrorBase.h"
+#include "SmartDashboard/NamedSendable.h"
+#include <set>
+#include <string>
+
+class CommandGroup;
+class Subsystem;
+
+/**
+ * The Command class is at the very core of the entire command framework.
+ * Every command can be started with a call to {@link Command#Start() Start()}.
+ * Once a command is started it will call {@link Command#Initialize() Initialize()}, and then
+ * will repeatedly call {@link Command#Execute() Execute()} until the {@link Command#IsFinished() IsFinished()}
+ * returns true. Once it does, {@link Command#End() End()} will be called.
+ *
+ * <p>However, if at any point while it is running {@link Command#Cancel() Cancel()} is called, then
+ * the command will be stopped and {@link Command#Interrupted() Interrupted()} will be called.</p>
+ *
+ * <p>If a command uses a {@link Subsystem}, then it should specify that it does so by
+ * calling the {@link Command#Requires(Subsystem) Requires(...)} method
+ * in its constructor. Note that a Command may have multiple requirements, and
+ * {@link Command#Requires(Subsystem) Requires(...)} should be
+ * called for each one.</p>
+ *
+ * <p>If a command is running and a new command with shared requirements is started,
+ * then one of two things will happen. If the active command is interruptible,
+ * then {@link Command#Cancel() Cancel()} will be called and the command will be removed
+ * to make way for the new one. If the active command is not interruptible, the
+ * other one will not even be started, and the active one will continue functioning.</p>
+ *
+ * @see CommandGroup
+ * @see Subsystem
+ */
+class Command : public ErrorBase, public NamedSendable, public ITableListener
+{
+ friend class CommandGroup;
+ friend class Scheduler;
+public:
+ Command();
+ Command(const char *name);
+ Command(double timeout);
+ Command(const char *name, double timeout);
+ virtual ~Command();
+ double TimeSinceInitialized();
+ void Requires(Subsystem *s);
+ bool IsCanceled();
+ void Start();
+ bool Run();
+ void Cancel();
+ bool IsRunning();
+ bool IsInterruptible();
+ void SetInterruptible(bool interruptible);
+ bool DoesRequire(Subsystem *subsystem);
+ typedef std::set<Subsystem *> SubsystemSet;
+ SubsystemSet GetRequirements();
+ CommandGroup *GetGroup();
+ void SetRunWhenDisabled(bool run);
+ bool WillRunWhenDisabled();
+
+
+protected:
+ void SetTimeout(double timeout);
+ bool IsTimedOut();
+ bool AssertUnlocked(const char *message);
+ void SetParent(CommandGroup *parent);
+ /**
+ * The initialize method is called the first time this Command is run after
+ * being started.
+ */
+ virtual void Initialize() = 0;
+ /**
+ * The execute method is called repeatedly until this Command either finishes
+ * or is canceled.
+ */
+ virtual void Execute() = 0;
+ /**
+ * Returns whether this command is finished.
+ * If it is, then the command will be removed
+ * and {@link Command#end() end()} will be called.
+ *
+ * <p>It may be useful for a team to reference the {@link Command#isTimedOut() isTimedOut()} method
+ * for time-sensitive commands.</p>
+ * @return whether this command is finished.
+ * @see Command#isTimedOut() isTimedOut()
+ */
+ virtual bool IsFinished() = 0;
+ /**
+ * Called when the command ended peacefully. This is where you may want
+ * to wrap up loose ends, like shutting off a motor that was being used
+ * in the command.
+ */
+ virtual void End() = 0;
+ /**
+ * Called when the command ends because somebody called {@link Command#cancel() cancel()}
+ * or another command shared the same requirements as this one, and booted
+ * it out.
+ *
+ * <p>This is where you may want
+ * to wrap up loose ends, like shutting off a motor that was being used
+ * in the command.</p>
+ *
+ * <p>Generally, it is useful to simply call the {@link Command#end() end()} method
+ * within this method</p>
+ */
+ virtual void Interrupted() = 0;
+ virtual void _Initialize();
+ virtual void _Interrupted();
+ virtual void _Execute();
+ virtual void _End();
+ virtual void _Cancel();
+
+private:
+ void InitCommand(const char *name, double timeout);
+ void LockChanges();
+ /*synchronized*/ void Removed();
+ void StartRunning();
+ void StartTiming();
+
+ /** The name of this command */
+ std::string m_name;
+ /** The time since this command was initialized */
+ double m_startTime;
+ /** The time (in seconds) before this command "times out" (or -1 if no timeout) */
+ double m_timeout;
+ /** Whether or not this command has been initialized */
+ bool m_initialized;
+ /** The requirements (or null if no requirements) */
+ SubsystemSet m_requirements;
+ /** Whether or not it is running */
+ bool m_running;
+ /** Whether or not it is interruptible*/
+ bool m_interruptible;
+ /** Whether or not it has been canceled */
+ bool m_canceled;
+ /** Whether or not it has been locked */
+ bool m_locked;
+ /** Whether this command should run when the robot is disabled */
+ bool m_runWhenDisabled;
+ /** The {@link CommandGroup} this is in */
+ CommandGroup *m_parent;
+
+
+public:
+ virtual std::string GetName();
+ virtual void InitTable(ITable* table);
+ virtual ITable* GetTable();
+ virtual std::string GetSmartDashboardType();
+ virtual void ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew);
+protected:
+ ITable* m_table;
+};
+
+#endif
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroup.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroup.cpp
new file mode 100644
index 0000000..fcf6d41
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroup.cpp
@@ -0,0 +1,383 @@
+/*----------------------------------------------------------------------------*/
+/* 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();
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroup.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroup.h
new file mode 100644
index 0000000..a081381
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroup.h
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __COMMAND_GROUP_H__
+#define __COMMAND_GROUP_H__
+
+#include "Commands/Command.h"
+#include "Commands/CommandGroupEntry.h"
+#include <list>
+#include <vector>
+
+/**
+ * A {@link CommandGroup} is a list of commands which are executed in sequence.
+ *
+ * <p>Commands in a {@link CommandGroup} are added using the {@link CommandGroup#AddSequential(Command) AddSequential(...)} method
+ * and are called sequentially.
+ * {@link CommandGroup CommandGroups} are themselves {@link Command Commands}
+ * and can be given to other {@link CommandGroup CommandGroups}.</p>
+ *
+ * <p>{@link CommandGroup CommandGroups} will carry all of the requirements of their {@link Command subcommands}. Additional
+ * requirements can be specified by calling {@link CommandGroup#Requires(Subsystem) Requires(...)}
+ * normally in the constructor.</P>
+ *
+ * <p>CommandGroups can also execute commands in parallel, simply by adding them
+ * using {@link CommandGroup#AddParallel(Command) AddParallel(...)}.</p>
+ *
+ * @see Command
+ * @see Subsystem
+ */
+class CommandGroup : public Command
+{
+public:
+ CommandGroup();
+ CommandGroup(const char *name);
+ virtual ~CommandGroup();
+
+ void AddSequential(Command *command);
+ void AddSequential(Command *command, double timeout);
+ void AddParallel(Command *command);
+ void AddParallel(Command *command, double timeout);
+ bool IsInterruptible();
+ int GetSize();
+
+protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+ virtual void _Initialize();
+ virtual void _Interrupted();
+ virtual void _Execute();
+ virtual void _End();
+
+private:
+ void CancelConflicts(Command *command);
+
+ typedef std::vector<CommandGroupEntry> CommandVector;
+ /** The commands in this group (stored in entries) */
+ CommandVector m_commands;
+ typedef std::list<CommandGroupEntry> CommandList;
+ /** The active children in this group (stored in entries) */
+ CommandList m_children;
+ /** The current command, -1 signifies that none have been run */
+ int m_currentCommandIndex;
+};
+
+#endif
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroupEntry.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroupEntry.cpp
new file mode 100644
index 0000000..1f420a0
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroupEntry.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroupEntry.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroupEntry.h
new file mode 100644
index 0000000..b7aeb34
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/CommandGroupEntry.h
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __COMMAND_GROUP_ENTRY_H__
+#define __COMMAND_GROUP_ENTRY_H__
+
+class Command;
+
+class CommandGroupEntry
+{
+public:
+ typedef enum {kSequence_InSequence, kSequence_BranchPeer, kSequence_BranchChild} Sequence;
+
+ CommandGroupEntry();
+ CommandGroupEntry(Command *command, Sequence state);
+ CommandGroupEntry(Command *command, Sequence state, double timeout);
+ bool IsTimedOut();
+
+ double m_timeout;
+ Command *m_command;
+ Sequence m_state;
+};
+
+#endif
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDCommand.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDCommand.cpp
new file mode 100644
index 0000000..4287e4c
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDCommand.cpp
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* 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);
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDCommand.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDCommand.h
new file mode 100644
index 0000000..4e45064
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDCommand.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __PID_COMMAND_H__
+#define __PID_COMMAND_H__
+
+#include "Commands/Command.h"
+#include "PIDSource.h"
+#include "PIDOutput.h"
+
+class PIDController;
+
+class PIDCommand : public Command, public PIDOutput, public PIDSource
+{
+public:
+ PIDCommand(const char *name, double p, double i, double d);
+ PIDCommand(const char *name, double p, double i, double d, double period);
+ PIDCommand(const char *name, double p, double i, double d, double f, double perioid);
+ PIDCommand(double p, double i, double d);
+ PIDCommand(double p, double i, double d, double period);
+ PIDCommand(double p, double i, double d, double f, double period);
+ virtual ~PIDCommand();
+
+ void SetSetpointRelative(double deltaSetpoint);
+
+ // PIDOutput interface
+ virtual void PIDWrite(float output);
+
+ // PIDSource interface
+ virtual double PIDGet();
+protected:
+ PIDController *GetPIDController();
+ virtual void _Initialize();
+ virtual void _Interrupted();
+ virtual void _End();
+ void SetSetpoint(double setpoint);
+ double GetSetpoint();
+ double GetPosition();
+
+ virtual double ReturnPIDInput() = 0;
+ virtual void UsePIDOutput(double output) = 0;
+
+private:
+ /** The internal {@link PIDController} */
+ PIDController *m_controller;
+
+public:
+ virtual void InitTable(ITable* table);
+ virtual std::string GetSmartDashboardType();
+};
+
+#endif
+
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDSubsystem.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDSubsystem.cpp
new file mode 100644
index 0000000..1023135
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDSubsystem.cpp
@@ -0,0 +1,237 @@
+/*----------------------------------------------------------------------------*/
+/* 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/PIDSubsystem.h"
+#include "PIDController.h"
+#include "float.h"
+
+// XXX max and min are not used?
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d) :
+ Subsystem(name)
+{
+ m_controller = new PIDController(p, i, d, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ */
+PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d, double f) :
+ Subsystem(name)
+{
+ m_controller = new PIDController(p, i, d, f, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values. It will also space the time
+ * between PID loop calculations to be equal to the given period.
+ * @param name the name
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedfoward value
+ * @param period the time (in seconds) between calculations
+ */
+PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d, double f,
+ double period) :
+ Subsystem(name)
+{
+ m_controller = new PIDController(p, i, d, f, this, this, period);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+ * It will use the class name as its name.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d) :
+ Subsystem("PIDSubsystem")
+{
+ m_controller = new PIDController(p, i, d, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+ * It will use the class name as its name.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f) :
+ Subsystem("PIDSubsystem")
+{
+ m_controller = new PIDController(p, i, d, f, this, this);
+}
+
+/**
+ * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+ * It will use the class name as its name.
+ * It will also space the time
+ * between PID loop calculations to be equal to the given period.
+ * @param p the proportional value
+ * @param i the integral value
+ * @param d the derivative value
+ * @param f the feedforward value
+ * @param period the time (in seconds) between calculations
+ */
+PIDSubsystem::PIDSubsystem(double p, double i, double d, double f, double period) :
+ Subsystem("PIDSubsystem")
+{
+ m_controller = new PIDController(p, i, d, f, this, this, period);
+}
+
+PIDSubsystem::~PIDSubsystem()
+{
+ delete m_controller;
+}
+
+/**
+ * Enables the internal {@link PIDController}
+ */
+void PIDSubsystem::Enable()
+{
+ m_controller->Enable();
+}
+
+/**
+ * Disables the internal {@link PIDController}
+ */
+void PIDSubsystem::Disable()
+{
+ m_controller->Disable();
+}
+
+
+/**
+ * Returns the {@link PIDController} used by this {@link PIDSubsystem}.
+ * Use this if you would like to fine tune the pid loop.
+ *
+ * @return the {@link PIDController} used by this {@link PIDSubsystem}
+ */
+PIDController *PIDSubsystem::GetPIDController()
+{
+ return m_controller;
+}
+
+/**
+ * Sets the setpoint to the given value. If {@link PIDCommand#SetRange(double, double) SetRange(...)}
+ * was called,
+ * then the given setpoint
+ * will be trimmed to fit within the range.
+ * @param setpoint the new setpoint
+ */
+void PIDSubsystem::SetSetpoint(double setpoint)
+{
+ m_controller->SetSetpoint(setpoint);
+}
+
+/**
+ * Adds the given value to the setpoint.
+ * If {@link PIDCommand#SetRange(double, double) SetRange(...)} was used,
+ * then the bounds will still be honored by this method.
+ * @param deltaSetpoint the change in the setpoint
+ */
+void PIDSubsystem::SetSetpointRelative(double deltaSetpoint)
+{
+ SetSetpoint(GetSetpoint() + deltaSetpoint);
+}
+
+/**
+ * Return the current setpoint
+ * @return The current setpoint
+ */
+double PIDSubsystem::GetSetpoint()
+{
+ return m_controller->GetSetpoint();
+}
+
+/**
+ * Sets the maximum and minimum values expected from the input.
+ *
+ * @param minimumInput the minimum value expected from the input
+ * @param maximumInput the maximum value expected from the output
+ */
+void PIDSubsystem::SetInputRange(float minimumInput, float maximumInput)
+{
+ m_controller->SetInputRange(minimumInput, maximumInput);
+}
+
+/*
+ * Set the absolute error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDSubsystem::SetAbsoluteTolerance(float absValue) {
+ m_controller->SetAbsoluteTolerance(absValue);
+}
+
+/*
+ * Set the percentage error which is considered tolerable for use with
+ * OnTarget.
+ * @param percentage error which is tolerable
+ */
+void PIDSubsystem::SetPercentTolerance(float percent) {
+ m_controller->SetPercentTolerance(percent);
+}
+
+/*
+ * Return true if the error is within the percentage of the total input range,
+ * determined by SetTolerance. This asssumes that the maximum and minimum input
+ * were set using SetInput. Use OnTarget() in the IsFinished() method of commands
+ * that use this subsystem.
+ *
+ * Currently this just reports on target as the actual value passes through the setpoint.
+ * Ideally it should be based on being within the tolerance for some period of time.
+ *
+ * @return true if the error is within the percentage tolerance of the input range
+ */
+bool PIDSubsystem::OnTarget()
+{
+ return m_controller->OnTarget();
+}
+
+/**
+ * Returns the current position
+ * @return the current position
+ */
+double PIDSubsystem::GetPosition()
+{
+ return ReturnPIDInput();
+}
+
+void PIDSubsystem::PIDWrite(float output)
+{
+ UsePIDOutput(output);
+}
+
+double PIDSubsystem::PIDGet()
+{
+ return ReturnPIDInput();
+}
+
+
+std::string PIDSubsystem::GetSmartDashboardType(){
+ return "PIDCommand";
+}
+void PIDSubsystem::InitTable(ITable* table){
+ m_controller->InitTable(table);
+ Subsystem::InitTable(table);
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDSubsystem.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDSubsystem.h
new file mode 100644
index 0000000..cf69a03
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PIDSubsystem.h
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __PID_SUBSYSTEM_H__
+#define __PID_SUBSYSTEM_H__
+
+#include "Commands/Subsystem.h"
+#include "PIDController.h"
+#include "PIDSource.h"
+#include "PIDOutput.h"
+
+/**
+ * This class is designed to handle the case where there is a {@link Subsystem}
+ * which uses a single {@link PIDController} almost constantly (for instance,
+ * an elevator which attempts to stay at a constant height).
+ *
+ * <p>It provides some convenience methods to run an internal {@link PIDController}.
+ * It also allows access to the internal {@link PIDController} in order to give total control
+ * to the programmer.</p>
+ *
+ */
+class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource
+{
+public:
+ PIDSubsystem(const char *name, double p, double i, double d);
+ PIDSubsystem(const char *name, double p, double i, double d, double f);
+ PIDSubsystem(const char *name, double p, double i, double d, double f, double period);
+ PIDSubsystem(double p, double i, double d);
+ PIDSubsystem(double p, double i, double d, double f);
+ PIDSubsystem(double p, double i, double d, double f, double period);
+ virtual ~PIDSubsystem();
+
+ void Enable();
+ void Disable();
+
+ // PIDOutput interface
+ virtual void PIDWrite(float output);
+
+ // PIDSource interface
+ virtual double PIDGet();
+ void SetSetpoint(double setpoint);
+ void SetSetpointRelative(double deltaSetpoint);
+ void SetInputRange(float minimumInput, float maximumInput);
+ double GetSetpoint();
+ double GetPosition();
+
+ virtual void SetAbsoluteTolerance(float absValue);
+ virtual void SetPercentTolerance(float percent);
+ virtual bool OnTarget();
+
+protected:
+ PIDController *GetPIDController();
+
+ virtual double ReturnPIDInput() = 0;
+ virtual void UsePIDOutput(double output) = 0;
+
+private:
+ /** The internal {@link PIDController} */
+ PIDController *m_controller;
+
+public:
+ virtual void InitTable(ITable* table);
+ virtual std::string GetSmartDashboardType();
+};
+
+#endif
+
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PrintCommand.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PrintCommand.cpp
new file mode 100644
index 0000000..6aba044
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PrintCommand.cpp
@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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(m_message.c_str());
+}
+
+void PrintCommand::Execute()
+{
+}
+
+bool PrintCommand::IsFinished()
+{
+ return true;
+}
+
+void PrintCommand::End()
+{
+}
+
+void PrintCommand::Interrupted()
+{
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PrintCommand.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PrintCommand.h
new file mode 100644
index 0000000..14de9fc
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/PrintCommand.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __PRINT_COMMAND_H__
+#define __PRINT_COMMAND_H__
+
+#include "Commands/Command.h"
+#include <string>
+
+class PrintCommand : public Command
+{
+public:
+ PrintCommand(const char *message);
+ virtual ~PrintCommand() {}
+
+protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+
+private:
+ std::string m_message;
+};
+
+#endif
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Scheduler.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Scheduler.cpp
new file mode 100644
index 0000000..9cd67d6
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Scheduler.cpp
@@ -0,0 +1,292 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "Synchronized.h"
+#include "WPIErrors.h"
+#include <iostream>
+#include <set>
+#include <semLib.h>
+#include <algorithm>
+
+Scheduler *Scheduler::_instance = NULL;
+
+Scheduler::Scheduler() :
+ m_buttonsLock(NULL),
+ m_additionsLock(NULL),
+ m_adding(false)
+{
+ m_buttonsLock = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE);
+ m_additionsLock = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE);
+
+ nUsageReporting::report(nUsageReporting::kResourceType_Command, nUsageReporting::kCommand_Scheduler);
+
+ m_enabled = true;
+}
+
+Scheduler::~Scheduler()
+{
+ semTake(m_additionsLock, WAIT_FOREVER);
+ semDelete(m_additionsLock);
+
+ semTake(m_buttonsLock, WAIT_FOREVER);
+ semDelete(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();
+ }
+}
+
+/**
+ * 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();
+ }
+ }
+
+ // 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);
+ }
+ }
+
+ // 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());
+ }
+}
+
+void Scheduler::UpdateTable() {
+ CommandSet::iterator commandIter;
+ if (m_table != NULL) {
+ // Get the commands to cancel
+ m_table->RetrieveValue("Cancel", *toCancel);
+
+ for (commandIter = m_commands.begin(); commandIter != m_commands.end();) {
+ for (unsigned i = 0; i < toCancel->size(); i++) {
+ Command *c = *commandIter;
+ if ((unsigned)c == toCancel->get(i)) {
+ c->Cancel();
+ }
+ }
+ }
+ toCancel->setSize(0);
+ m_table->PutValue("Cancel", *toCancel);
+
+ commands->setSize(0);
+ ids->setSize(0);
+
+ // Set the running commands
+ for (commandIter = m_commands.begin(); commandIter != m_commands.end();) {
+ Command *c = *commandIter;
+ commands->add(c->GetName());
+ ids->add((unsigned)c);
+ }
+ 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;
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Scheduler.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Scheduler.h
new file mode 100644
index 0000000..2636ac6
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Scheduler.h
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __SCHEDULER_H__
+#define __SCHEDULER_H__
+
+#include "Commands/Command.h"
+#include "ErrorBase.h"
+#include "SmartDashboard/NamedSendable.h"
+#include "networktables/NetworkTable.h"
+#include "networktables2/type/NumberArray.h"
+#include "networktables2/type/StringArray.h"
+#include "SmartDashboard/SmartDashboard.h"
+#include <list>
+#include <map>
+#include <set>
+#include <vector>
+
+class ButtonScheduler;
+class Subsystem;
+
+class Scheduler : public ErrorBase, public NamedSendable
+{
+public:
+ static Scheduler *GetInstance();
+
+ void AddCommand(Command* command);
+ void AddButton(ButtonScheduler* button);
+ void RegisterSubsystem(Subsystem *subsystem);
+ void Run();
+ void Remove(Command *command);
+ void RemoveAll();
+ void SetEnabled(bool enabled);
+
+ void UpdateTable();
+ std::string GetSmartDashboardType();
+ void InitTable(ITable *subTable);
+ ITable * GetTable();
+ std::string GetName();
+ std::string GetType();
+
+private:
+ Scheduler();
+ virtual ~Scheduler();
+
+ void ProcessCommandAddition(Command *command);
+
+ static Scheduler *_instance;
+ Command::SubsystemSet m_subsystems;
+ SEM_ID m_buttonsLock;
+ typedef std::vector<ButtonScheduler *> ButtonVector;
+ ButtonVector m_buttons;
+ typedef std::vector<Command *> CommandVector;
+ SEM_ID m_additionsLock;
+ CommandVector m_additions;
+ typedef std::set<Command *> CommandSet;
+ CommandSet m_commands;
+ bool m_adding;
+ bool m_enabled;
+ StringArray *commands;
+ NumberArray *ids;
+ NumberArray *toCancel;
+ ITable *m_table;
+};
+#endif
+
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/StartCommand.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/StartCommand.cpp
new file mode 100644
index 0000000..9e1e9df
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/StartCommand.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/StartCommand.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/StartCommand.h
new file mode 100644
index 0000000..5e9cf72
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/StartCommand.h
@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __START_COMMAND_H__
+#define __START_COMMAND_H__
+
+#include "Commands/Command.h"
+
+class StartCommand : public Command
+{
+public:
+ StartCommand(Command *commandToStart);
+ virtual ~StartCommand() {}
+
+protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+
+private:
+ Command *m_commandToFork;
+};
+
+#endif
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Subsystem.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Subsystem.cpp
new file mode 100644
index 0000000..dfd559c
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Subsystem.cpp
@@ -0,0 +1,174 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+}
+/**
+ * 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;
+}
+
+/**
+ * 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_table != NULL)
+ {
+ if (m_currentCommand != NULL)
+ {
+ m_table->PutBoolean("hasCommand", true);
+ m_table->PutString("command", m_currentCommand->GetName());
+ }
+ else
+ {
+ m_table->PutBoolean("hasCommand", 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;
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Subsystem.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Subsystem.h
new file mode 100644
index 0000000..b04616d
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/Subsystem.h
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __SUBSYSTEM_H__
+#define __SUBSYSTEM_H__
+
+#include "ErrorBase.h"
+#include "SmartDashboard/NamedSendable.h"
+#include <string>
+
+
+class Command;
+
+class Subsystem : public ErrorBase, public NamedSendable
+{
+ friend class Scheduler;
+public:
+ Subsystem(const char *name);
+ virtual ~Subsystem() {}
+
+ void SetDefaultCommand(Command *command);
+ Command *GetDefaultCommand();
+ void SetCurrentCommand(Command *command);
+ Command *GetCurrentCommand();
+ virtual void InitDefaultCommand();
+
+private:
+ void ConfirmCommand();
+
+ Command *m_currentCommand;
+ Command *m_defaultCommand;
+ std::string m_name;
+ bool m_initializedDefaultCommand;
+
+public:
+ virtual std::string GetName();
+ virtual void InitTable(ITable* table);
+ virtual ITable* GetTable();
+ virtual std::string GetSmartDashboardType();
+protected:
+ ITable* m_table;
+};
+
+#endif
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitCommand.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitCommand.cpp
new file mode 100644
index 0000000..6276363
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitCommand.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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()
+{
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitCommand.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitCommand.h
new file mode 100644
index 0000000..ca9bca8
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitCommand.h
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __WAIT_COMMAND_H__
+#define __WAIT_COMMAND_H__
+
+#include "Commands/Command.h"
+
+class WaitCommand : public Command
+{
+public:
+ WaitCommand(double timeout);
+ WaitCommand(const char *name, double timeout);
+ virtual ~WaitCommand() {}
+
+protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+};
+
+#endif
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitForChildren.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitForChildren.cpp
new file mode 100644
index 0000000..07d03b3
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitForChildren.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitForChildren.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitForChildren.h
new file mode 100644
index 0000000..4d67d2a
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitForChildren.h
@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __WAIT_FOR_CHILDREN_H__
+#define __WAIT_FOR_CHILDREN_H__
+
+#include "Commands/Command.h"
+
+class WaitForChildren : public Command
+{
+public:
+ WaitForChildren(double timeout);
+ WaitForChildren(const char *name, double timeout);
+ virtual ~WaitForChildren() {}
+
+protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+};
+
+#endif
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitUntilCommand.cpp b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitUntilCommand.cpp
new file mode 100644
index 0000000..5a6b329
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitUntilCommand.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* 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/WaitUntilCommand.h"
+#include "DriverStation.h"
+#include "Timer.h"
+
+/**
+ * A {@link WaitCommand} will wait until a certain match time before finishing.
+ * This will wait until the game clock reaches some value, then continue to the
+ * next command.
+ * @see CommandGroup
+ */
+WaitUntilCommand::WaitUntilCommand(double time) :
+ Command("WaitUntilCommand", time)
+{
+ m_time = time;
+}
+
+WaitUntilCommand::WaitUntilCommand(const char *name, double time) :
+ Command(name, time)
+{
+ m_time = time;
+}
+
+void WaitUntilCommand::Initialize()
+{
+}
+
+void WaitUntilCommand::Execute()
+{
+}
+
+/**
+ * Check if we've reached the actual finish time.
+ */
+bool WaitUntilCommand::IsFinished()
+{
+ return DriverStation::GetInstance()->GetMatchTime() >= m_time;
+}
+
+void WaitUntilCommand::End()
+{
+}
+
+void WaitUntilCommand::Interrupted()
+{
+}
diff --git a/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitUntilCommand.h b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitUntilCommand.h
new file mode 100644
index 0000000..a624705
--- /dev/null
+++ b/azaleasource/WPILibCProgramming/trunk/WPILib/Commands/WaitUntilCommand.h
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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. */
+/*----------------------------------------------------------------------------*/
+
+#ifndef __WAIT_UNTIL_COMMAND_H__
+#define __WAIT_UNTIL_COMMAND_H__
+
+#include "Commands/Command.h"
+
+class WaitUntilCommand : public Command
+{
+public:
+ WaitUntilCommand(double time);
+ WaitUntilCommand(const char *name, double time);
+ virtual ~WaitUntilCommand() {}
+
+protected:
+ virtual void Initialize();
+ virtual void Execute();
+ virtual bool IsFinished();
+ virtual void End();
+ virtual void Interrupted();
+
+private:
+ double m_time;
+};
+
+#endif