BaseSubsystem.h

#pragma once
 
#include <memory>
#include <string>
 
#include <frc/DriverStation.h>
#include <frc/TimedRobot.h>
#include <frc/Timer.h>
#include <frc2/command/SubsystemBase.h>
#include <networktables/NetworkTable.h>
#include <networktables/NetworkTableEntry.h>
#include <networktables/NetworkTableInstance.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableHelper.h>
 
#include "valkyrie/Gamepad.h"
#include "valkyrie/Loggable.h"
#include "valkyrie/util/Profiler.h"
 
namespace valor {

class BaseSubsystem : public frc2::Subsystem, public valor::Loggable {
   public:
    explicit BaseSubsystem(std::string name) : Loggable{name}, operatorGamepad(nullptr), driverGamepad(nullptr) {
        frc2::CommandScheduler::GetInstance().RegisterSubsystem(this);
        profiler.Start();
    }

    virtual void AnalyzeDashboard() {}

    virtual void AssessInputs() {}

    virtual void AssignOutputs() {}

    virtual void Reset() {}
 
    void SetGamepads(valor::Gamepad* _operatorGamepad, valor::Gamepad* _driverGamepad) {
        operatorGamepad = _operatorGamepad;
        driverGamepad = _driverGamepad;
    }
 
   protected:
    valor::Gamepad* operatorGamepad;
    valor::Gamepad* driverGamepad;
 
   private:
    void Periodic() {
        profiler.Reset();
        if (frc::DriverStation::IsTeleopEnabled())
            AssessInputs();
        units::millisecond_t assessInputsTime = profiler.Get();
 
        AnalyzeDashboard();
        units::millisecond_t analyzeDashboardTime = profiler.Get() - assessInputsTime;
 
        if (frc::DriverStation::IsEnabled())
            AssignOutputs();
        units::millisecond_t assignOutputsTime = profiler.Get() - analyzeDashboardTime;
        if (util::Profiler::IsEnabled()) {
            WriteLog("Profiler/Cycle Time", units::millisecond_t{profiler.Get()});
            WriteLog("Profiler/Assign Outputs Time", assignOutputsTime);
            WriteLog("Profiler/Assess Inputs Time", assessInputsTime);
            WriteLog("Profiler/Analyze Dashboard Time", analyzeDashboardTime);
        }
    }
 
    frc::Timer profiler;
};
}  // namespace valor