IterativeRobot.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 "IterativeRobot.h"

#include "DriverStation.h"
#include "NetworkCommunication/UsageReporting.h"
#include <taskLib.h>
#include "SmartDashboard/SmartDashboard.h"
#include "LiveWindow/LiveWindow.h"
#include "networktables/NetworkTable.h"

constexpr double IterativeRobot::kDefaultPeriod;

IterativeRobot::IterativeRobot()
        : m_disabledInitialized (false)
        , m_autonomousInitialized (false)
        , m_teleopInitialized (false)
        , m_testInitialized (false)
        , m_period (kDefaultPeriod)
{
        m_watchdog.SetEnabled(false);
}

IterativeRobot::~IterativeRobot()
{
}

void IterativeRobot::SetPeriod(double period)
{
        if (period > 0.0)
        {
                // Not syncing with the DS, so start the timer for the main loop
                m_mainLoopTimer.Reset();
                m_mainLoopTimer.Start();
        }
        else
        {
                // Syncing with the DS, don't need the timer
                m_mainLoopTimer.Stop();
        }
        m_period = period;
}

double IterativeRobot::GetPeriod()
{
        return m_period;
}

double IterativeRobot::GetLoopsPerSec()
{
        // If syncing to the driver station, we don't know the rate,
        //   so guess something close.
        if (m_period <= 0.0)
                return 50.0;
        return 1.0 / m_period;
}

void IterativeRobot::StartCompetition()
{
        nUsageReporting::report(nUsageReporting::kResourceType_Framework, nUsageReporting::kFramework_Iterative);

        LiveWindow *lw = LiveWindow::GetInstance();
        // first and one-time initialization
        SmartDashboard::init();
        NetworkTable::GetTable("LiveWindow")->GetSubTable("~STATUS~")->PutBoolean("LW Enabled", false);
        RobotInit();

        // loop forever, calling the appropriate mode-dependent function
        lw->SetEnabled(false);
        while (true)
        {
                // Call the appropriate function depending upon the current robot mode
                if (IsDisabled())
                {
                        // call DisabledInit() if we are now just entering disabled mode from
                        // either a different mode or from power-on
                        if(!m_disabledInitialized)
                        {
                                lw->SetEnabled(false);
                                DisabledInit();
                                m_disabledInitialized = true;
                                // reset the initialization flags for the other modes
                                m_autonomousInitialized = false;
                m_teleopInitialized = false;
                m_testInitialized = false;
                        }
                        if (NextPeriodReady())
                        {
                                FRC_NetworkCommunication_observeUserProgramDisabled();
                                DisabledPeriodic();
                        }
                }
                else if (IsAutonomous())
                {
                        // call AutonomousInit() if we are now just entering autonomous mode from
                        // either a different mode or from power-on
                        if(!m_autonomousInitialized)
                        {
                                lw->SetEnabled(false);
                                AutonomousInit();
                                m_autonomousInitialized = true;
                                // reset the initialization flags for the other modes
                                m_disabledInitialized = false;
                m_teleopInitialized = false;
                m_testInitialized = false;
                        }
                        if (NextPeriodReady())
                        {
                                FRC_NetworkCommunication_observeUserProgramAutonomous();
                                AutonomousPeriodic();
                        }
                }
        else if (IsTest())
        {
            // call TestInit() if we are now just entering test mode from
            // either a different mode or from power-on
            if(!m_testInitialized)
            {
                lw->SetEnabled(true);
                TestInit();
                m_testInitialized = true;
                // reset the initialization flags for the other modes
                m_disabledInitialized = false;
                m_autonomousInitialized = false;
                m_teleopInitialized = false;
            }
            if (NextPeriodReady())
            {
                FRC_NetworkCommunication_observeUserProgramTest();
                TestPeriodic();
            }
        }
                else
                {
                        // call TeleopInit() if we are now just entering teleop mode from
                        // either a different mode or from power-on
                        if(!m_teleopInitialized)
                        {
                                lw->SetEnabled(false);
                                TeleopInit();
                                m_teleopInitialized = true;
                                // reset the initialization flags for the other modes
                                m_disabledInitialized = false;
                m_autonomousInitialized = false;
                m_testInitialized = false;
                Scheduler::GetInstance()->SetEnabled(true);
                        }
                        if (NextPeriodReady())
                        {
                                FRC_NetworkCommunication_observeUserProgramTeleop();
                                TeleopPeriodic();
                        }
                }
                // wait for driver station data so the loop doesn't hog the CPU
                m_ds->WaitForData();
        }       
}

bool IterativeRobot::NextPeriodReady()
{
        if (m_period > 0.0)
        {
                return m_mainLoopTimer.HasPeriodPassed(m_period);
        }
        else
        {
                return m_ds->IsNewControlData();
        }
}

void IterativeRobot::RobotInit()
{
        printf("Default %s() method... Overload me!\n", __FUNCTION__);
}

void IterativeRobot::DisabledInit()
{
        printf("Default %s() method... Overload me!\n", __FUNCTION__);
}

void IterativeRobot::AutonomousInit()
{
        printf("Default %s() method... Overload me!\n", __FUNCTION__);
}

void IterativeRobot::TeleopInit()
{
    printf("Default %s() method... Overload me!\n", __FUNCTION__);
}

void IterativeRobot::TestInit()
{
    printf("Default %s() method... Overload me!\n", __FUNCTION__);
}

void IterativeRobot::DisabledPeriodic()
{
        static bool firstRun = true;
        if (firstRun)
        {
                printf("Default %s() method... Overload me!\n", __FUNCTION__);
                firstRun = false;
        }
        taskDelay(1);
}

void IterativeRobot::AutonomousPeriodic()
{
        static bool firstRun = true;
        if (firstRun)
        {
                printf("Default %s() method... Overload me!\n", __FUNCTION__);
                firstRun = false;
        }
        taskDelay(1);
}

void IterativeRobot::TeleopPeriodic()
{
        static bool firstRun = true;
        if (firstRun)
        {
                printf("Default %s() method... Overload me!\n", __FUNCTION__);
                firstRun = false;
        }
        taskDelay(1);
}

void IterativeRobot::TestPeriodic()
{
    static bool firstRun = true;
    if (firstRun)
    {
        printf("Default %s() method... Overload me!\n", __FUNCTION__);
        firstRun = false;
    }
    taskDelay(1);
}