RobotBase.cpp

#include "RobotBase.hpp"
#include <ctre/phoenix6/unmanaged/Unmanaged.hpp>

int RobotBase::Run()
{
    printf("Robot program startup complete\n");

    while (IsRunning()) {
        auto const start = std::chrono::steady_clock::now();

        /* run the robot periodic function */
        RobotPeriodic();

        /* check if we're enabled */
        if (IsEnabled()) {
            /* enabled */
            if (!_lastEnabled.value_or(false)) {
                /* just switched, run enabled init */
                printf("Robot ENABLED\n");
                EnabledInit();
                _lastEnabled = true;
            }

            /* enable for 100 ms */
            ctre::phoenix::unmanaged::FeedEnable(100);
            /* run enabled periodic */
            EnabledPeriodic();
        } else {
            /* disabled */
            if (_lastEnabled.value_or(true)) {
                /* just switched, run disabled init */
                printf("Robot DISABLED\n");
                DisabledInit();

                if (_lastEnabled.has_value()) {
                    /* we were enabled, disable now */
                    ctre::phoenix::unmanaged::FeedEnable(0);
                }
                _lastEnabled = false;
            }

            /* run disabled periodic */
            DisabledPeriodic();
        }

        auto const end = std::chrono::steady_clock::now();

        /* yield for the remainder of the loop time */
        units::millisecond_t const dtMs{std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / 1000.0};
        if (dtMs < _loopTime) {
            SleepFor(_loopTime - dtMs);
        } else {
            /* loop overrun */
            ReportLoopOverrun(dtMs);
            /* yield control of this thread */
            SleepFor(0_ms);
        }
    }
    return 0;
}

void RobotBase::ReportLoopOverrun(units::millisecond_t measured)
{
    auto const now = std::chrono::steady_clock::now();
    auto const dtMs = std::chrono::duration_cast<std::chrono::milliseconds>(now - _lastErrorTime).count();

    if (dtMs > kErrorTimeMs) {
        fprintf(stderr, "Warning: %.1fms loop time overrun\n"
                "    Robot loop took %.3fms\n",
                _loopTime.value(), measured.value());
        _lastErrorTime = now;
    }
}