ramp.hpp

#pragma once
#include <cmath>
#include <concepts>
#include <chrono>
 
#include "utils/controller.hpp"
 
namespace roboctrl::utils{
 
 
template<std::floating_point T>
class ramp {
public:
    struct params_type {
        T acc; 
    };
 
    using input_type = T;  
    using state_type = T;  
 
public:
    ramp() = default;
 
    explicit ramp(const params_type& params)
        : acc_{params.acc}, last_update_{std::chrono::steady_clock::now()} {}
 
    inline void update(T target) noexcept {
        using namespace std::chrono;
        const auto now = steady_clock::now();
        const std::chrono::duration<T> dur = now - last_update_; 
        last_update_ = now;
 
        const T dt = dur.count();
        const T diff = target - out_;
        const T max_step = acc_ * dt;
 
        if (std::fabs(diff) <= max_step)
            out_ = target;
        else
            out_ += std::copysign(max_step, diff);
    }
 
    inline void set_acc(T new_acc) noexcept { acc_ = new_acc; }
 
    inline void reset() noexcept { out_ = T{0}; }
 
    inline void reset(T value) noexcept { out_ = value; }
 
    inline state_type state() const noexcept { return out_; }
 
private:
    T out_ = T{0}; 
    T acc_ = T{0}; 
    std::chrono::steady_clock::time_point last_update_{}; 
};
 
using ramp_f = ramp<float>;
 
using ramp_d = ramp<double>;
 
static_assert(utils::controller<ramp_f>);
static_assert(utils::controller<ramp_d>);
 
} // namespace control