BaseSensor.h

#pragma once
 
#include <functional>
#include <string>
 
#include <frc/TimedRobot.h>
#include <frc/Timer.h>
 
#include "valkyrie/Loggable.h"
#include "valkyrie/Robot.h"
#include "valkyrie/util/Profiler.h"
 
namespace valor {
namespace sensors {

class BaseSensorCore {
   public:
    static constexpr units::millisecond_t LOOP_PERIOD = frc::TimedRobot::kDefaultPeriod / 2;
 
    BaseSensorCore() {
        valor::Robot::GetRobot().AddPeriodic(
            [this] {
                Refresh();
                Calculate();
            },
            LOOP_PERIOD, 5_ms);
    }
 
    virtual void Refresh() = 0;
 
    virtual void Calculate() {}
};

template <class T>
class BaseSensor : public virtual BaseSensorCore, 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() const { return currState; }

    void Refresh() override {
        prevState = currState;
        if (sensorLambda) {
            currState = sensorLambda();
            if (postProcessor)
                currState = postProcessor(currState);
        }
    }
 
   protected:
    std::function<T()> sensorLambda;

    std::function<T(T)> postProcessor;

    T prevState, currState;
};
 
}  // namespace sensors
}  // namespace valor