RobotBase.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                                                         */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "RobotBase.h"

#include "DriverStation.h"
#include "NetworkCommunication/FRCComm.h"
#include "NetworkCommunication/symModuleLink.h"
#include "NetworkCommunication/UsageReporting.h"
#include "Utility.h"
#include <moduleLib.h>
#include <taskLib.h>
#include <unldLib.h>
#include <cstring>

RobotBase* RobotBase::m_instance = NULL;

const char *FILE_NAME = "/c/FRC_Lib_Version.ini";
const char *VERSION_STRING = "C++ 2014 Update 0";


void RobotBase::setInstance(RobotBase* robot)
{
        wpi_assert(m_instance == NULL);
        m_instance = robot;
}

RobotBase &RobotBase::getInstance()
{
        return *m_instance;
}

RobotBase::RobotBase()
        : m_task (NULL)
        , m_ds (NULL)
{
        m_ds = DriverStation::GetInstance();
}

RobotBase::~RobotBase()
{
        SensorBase::DeleteSingletons();
        delete m_task;
        m_task = NULL;
        m_instance = NULL;
}

bool RobotBase::IsSystemActive()
{
        return m_watchdog.IsSystemActive();
}

Watchdog &RobotBase::GetWatchdog()
{
        return m_watchdog;
}

bool RobotBase::IsEnabled()
{
        return m_ds->IsEnabled();
}

bool RobotBase::IsDisabled()
{
        return m_ds->IsDisabled();
}

bool RobotBase::IsAutonomous()
{
        return m_ds->IsAutonomous();
}

bool RobotBase::IsOperatorControl()
{
        return m_ds->IsOperatorControl();
}

bool RobotBase::IsTest()
{
    return m_ds->IsTest();
}

bool RobotBase::IsNewDataAvailable()
{
        return m_ds->IsNewControlData();
}

void RobotBase::robotTask(FUNCPTR factory, Task *task)
{
        RobotBase::setInstance((RobotBase*)factory());
        RobotBase::getInstance().m_task = task;
        RobotBase::getInstance().StartCompetition();
}

void RobotBase::WriteVersionString() {
        FILE *file = fopen(FILE_NAME, "w");
        if (file != NULL) {
                fputs(VERSION_STRING, file);
                fclose(file);
        }
}

void RobotBase::startRobotTask(FUNCPTR factory)
{
#ifdef SVN_REV
        if (strlen(SVN_REV))
        {
                printf("WPILib was compiled from SVN revision %s\n", SVN_REV);
        }
        else
        {
                printf("WPILib was compiled from a location that is not source controlled.\n");
        }
#else
        printf("WPILib was compiled without -D'SVN_REV=nnnn'\n");
#endif

        // Check for startup code already running
        int32_t oldId = taskNameToId(const_cast<char*>("FRC_RobotTask"));
        if (oldId != ERROR)
        {
                // Find the startup code module.
                char moduleName[256];
                moduleNameFindBySymbolName("FRC_UserProgram_StartupLibraryInit", moduleName);
                MODULE_ID startupModId = moduleFindByName(moduleName);
                if (startupModId != NULL)
                {
                        // Remove the startup code.
                        unldByModuleId(startupModId, 0);
                        printf("!!!   Error: Default code was still running... It was unloaded for you... Please try again.\n");
                        return;
                }
                // This case should no longer get hit.
                printf("!!!   Error: Other robot code is still running... Unload it and then try again.\n");
                return;
        }

        // Let the framework know that we are starting a new user program so the Driver Station can disable.
        FRC_NetworkCommunication_observeUserProgramStarting();

        // Let the Usage Reporting framework know that there is a C++ program running
        nUsageReporting::report(nUsageReporting::kResourceType_Language, nUsageReporting::kLanguage_CPlusPlus);
        
        RobotBase::WriteVersionString();

        // Start robot task
        // This is done to ensure that the C++ robot task is spawned with the floating point
        // context save parameter.
        Task *task = new Task("RobotTask", (FUNCPTR)RobotBase::robotTask, Task::kDefaultPriority, 64000);
        task->Start((int32_t)factory, (int32_t)task);
}

class RobotDeleter
{
public:
        RobotDeleter() {}
        ~RobotDeleter()
        {
                delete &RobotBase::getInstance();
        }
};
static RobotDeleter g_robotDeleter;