pid.h

Go to the documentation of this file.

 
#pragma once
 
#include "./min_max.h"
 
#ifdef EMLABCPP_USE_NLOHMANN_JSON
#include <nlohmann/json.hpp>
#endif
 
namespace emlabcpp
{
 
struct pid_coefficients
{
        float p = 0;
        float i = 0;
        float d = 0;
};
 
struct pid_config
{
        pid_coefficients coefficients{ .p = 1.f, .i = 0.f, .d = 0.f };
 
        min_max< float > limits{ 0.f, 100.f };
};
 
//
template < typename TimeType >
struct pid
{
        using time_type = TimeType;
        using config    = pid_config;
 
        config    cfg;
        float     i_sum         = 0;
        float     last_measured = 0;
        time_type last_time;
        float     output;
 
        pid( time_type now, config const& conf = config{} )
          : cfg( conf )
          , last_time( now )
          , output( clamp( 0.f, cfg.limits ) )
        {
        }
};
 
template < typename TimeType >
void update_limits( pid< TimeType >& pid, min_max< float > lim )
{
        pid.cfg.limits = lim;
        pid.output     = clamp( pid.output, pid.cfg.limits );
        pid.i_sum      = clamp( pid.i_sum, pid.cfg.limits );
}
 
template < typename TimeType >
void update_output( pid< TimeType >& pid, float output )
{
        pid.output = clamp( output, pid.cfg.limits );
}
 
template < typename TimeType >
float update( pid< TimeType >& pid, TimeType now, float measured, float desired )
{
        if ( now == pid.last_time )
                return pid.output;
 
        auto t_diff = static_cast< float >( now - pid.last_time );
 
        pid_coefficients const& coeff = pid.cfg.coefficients;
 
        float const error = desired - measured;
        pid.i_sum += coeff.i * ( error * t_diff );
        pid.i_sum = clamp( pid.i_sum, pid.cfg.limits );
 
        float const measured_diff = ( measured - pid.last_measured ) / t_diff;
        pid.output                = coeff.p * error + pid.i_sum - coeff.d * measured_diff;
        pid.output                = clamp( pid.output, pid.cfg.limits );
 
        pid.last_measured = measured;
        pid.last_time     = now;
 
        return pid.output;
}
 
template < typename TimeType >
void reset( pid< TimeType >& pid, TimeType now, float last_measured )
{
        pid.last_time     = now;
        pid.last_measured = last_measured;
}
 
}  // namespace emlabcpp
 
#ifdef EMLABCPP_USE_NLOHMANN_JSON
 
template <>
struct nlohmann::adl_serializer< emlabcpp::pid_coefficients >
{
        using cfg_type = emlabcpp::pid_coefficients;
 
        static void to_json( nlohmann::json& j, cfg_type const& cfg )
        {
                j["p"] = cfg.p;
                j["i"] = cfg.i;
                j["d"] = cfg.d;
        }
 
        static cfg_type from_json( nlohmann::json const& j )
        {
                cfg_type cfg;
                cfg.p = j["p"];
                cfg.i = j["i"];
                cfg.d = j["d"];
                return cfg;
        }
};
 
template <>
struct nlohmann::adl_serializer< emlabcpp::pid_config >
{
        using cfg_type = emlabcpp::pid_config;
 
        static void to_json( nlohmann::json& j, cfg_type const& cfg )
        {
                j["coefficients"] = cfg.coefficients;
                j["limits"]       = cfg.limits;
        }
 
        static cfg_type from_json( nlohmann::json const& j )
        {
                cfg_type cfg;
                cfg.coefficients = j["coefficients"];
                cfg.limits       = j["limits"];
                return cfg;
        }
};
 
#endif