BaseSensor.h

 
#pragma once
 
#include <functional>
#include <string>
 
#include <frc/TimedRobot.h>
#include <frc/Timer.h>
 
#include "valkyrie/Loggable.h"
#include "valkyrie/util/Profiler.h"
 
namespace valor {
 
template <class T>
class BaseSensor : public valor::Loggable {
   public:
    virtual void reset() { prevState = currState = T{}; }
 
    void setGetter(std::function<T()> _lambda) { sensorLambda = _lambda; }
 
    void applyPostProcessing(std::function<T(T)> func) { postProcessor = func; }
 
    inline T get() { return currState; }
 
   protected:
    explicit BaseSensor(frc::TimedRobot& robot) {
        profiler.Start();
        robot.AddPeriodic(
            [this] {
                profiler.Reset();
                refresh();
                units::millisecond_t refreshTime = profiler.Get();
                calculate();
                units::millisecond_t calculateTime = profiler.Get() - refreshTime;
                if (util::Profiler::IsEnabled()) {
                    WriteLog("Profiler/Cycle Time", units::millisecond_t{profiler.Get()});
                    WriteLog("Profiler/Refresh Time", refreshTime);
                    WriteLog("Profiler/Calculate Time", calculateTime);
                }
            },
            frc::TimedRobot::kDefaultPeriod / 2, 5_ms);
    }
 
    BaseSensor() = default;
 
    virtual void calculate() = 0;
 
    virtual void refresh() {
        prevState = currState;
        if (sensorLambda) {
            currState = sensorLambda();
            if (postProcessor)
                currState = postProcessor(currState);
        }
    }
 
    std::function<T()> sensorLambda;
 
    std::function<T(T)> postProcessor;
 
    T prevState, currState;
 
    frc::Timer profiler;
};
}  // namespace valor