Subsystem.cpp

/*----------------------------------------------------------------------------*/
/* 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 "NetworkTables/NetworkTable.h"
#include "WPIErrors.h"

Subsystem::Subsystem(const char *name) :
        m_table(NULL),
        m_currentCommand(NULL),
        m_defaultCommand(NULL),
        m_initializedDefaultCommand(false)
{
        m_name = name;
        Scheduler::GetInstance()->RegisterSubsystem(this);
}
void Subsystem::InitDefaultCommand() {

}

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 != 0)
                {
                        m_table->PutBoolean("hasDefault", true);
                        m_table->PutSubTable("default", m_defaultCommand->GetTable());
                }
                else
                {
                        m_table->PutBoolean("hasDefault", false);
                }
        }
}

Command *Subsystem::GetDefaultCommand()
{
        if (!m_initializedDefaultCommand) {
                m_initializedDefaultCommand = true;
                InitDefaultCommand();
        }
        return m_defaultCommand;
}

void Subsystem::SetCurrentCommand(Command *command)
{
        m_currentCommand = command;
}

Command *Subsystem::GetCurrentCommand()
{
        return m_currentCommand;
}

void Subsystem::ConfirmCommand()
{
        if (m_table != NULL)
        {
                if (m_currentCommand != NULL)
                {
                        m_table->PutBoolean("hasCommand", true);
                        m_table->PutSubTable("command", m_currentCommand->GetTable());
                }
                else
                {
                        m_table->PutBoolean("hasCommand", false);
                }
        }
}

std::string Subsystem::GetName()
{
        return "Subsystem";
}

std::string Subsystem::GetType()
{
        return "Subsystem";
}

NetworkTable *Subsystem::GetTable()
{
        if (m_table == NULL)
        {
                m_table = new NetworkTable();
                m_table->PutInt("count", 0);
        }
        return m_table;
}