pid.hpp

#pragma once
 
#include <algorithm>
#include <cmath>
 
#include "cycle_value.hpp"
#include "libxr_def.hpp"
 
namespace LibXR
{
using DefaultScalar = LIBXR_DEFAULT_SCALAR;
 
constexpr DefaultScalar PID_SIGMA = 1e-6f;
 
template <typename Scalar = DefaultScalar>
class PID
{
 public:
  struct Param
  {
    Scalar k = 1.0;          
    Scalar p = 0.0;          
    Scalar i = 0.0;          
    Scalar d = 0.0;          
    Scalar i_limit = 0.0;    
    Scalar out_limit = 0.0;  
    bool cycle = false;      
  };
 
  template <typename Param>
  PID(Param&& PARAM) : param_(std::forward<Param>(PARAM))
  {
    Reset();
  }
 
  Scalar Calculate(Scalar sp, Scalar fb, Scalar dt)
  {
    if (!std::isfinite(sp) || !std::isfinite(fb) || !std::isfinite(dt))
    {
      return last_out_;
    }
 
    // Compute error
    Scalar err = param_.cycle ? CycleValue<Scalar>(sp) - fb : sp - fb;
    Scalar k_err = err * param_.k;
 
    // Derivative from feedback change
    fb *= param_.k;
    Scalar d = (fb - last_fb_) / dt;
    if (!std::isfinite(d))
    {
      d = 0;
    }
 
    // Compute PD
    Scalar output = (k_err * param_.p) - (d * param_.d);
 
    // Integrate if within limits
    Scalar i_term = i_ + k_err * dt;
    Scalar i_out = i_term * param_.i;
 
    if (param_.i > PID_SIGMA && std::isfinite(i_term))
    {
      if (std::abs(output + i_out) <= param_.out_limit &&
          std::abs(i_term) <= param_.i_limit)
      {
        i_ = i_term;
      }
    }
 
    // Apply output limits
    output += i_out;
    output += feed_forward_;
    if (std::isfinite(output) && param_.out_limit > PID_SIGMA)
    {
      output = std::clamp(output, -param_.out_limit, param_.out_limit);
    }
 
    // Store states
    last_err_ = err;
    last_fb_ = fb;
    last_out_ = output;
    last_der_ = d;
    return output;
  }
 
  Scalar Calculate(Scalar sp, Scalar fb, Scalar fb_dot, Scalar dt)
  {
    if (!std::isfinite(sp) || !std::isfinite(fb) || !std::isfinite(fb_dot) ||
        !std::isfinite(dt))
    {
      return last_out_;
    }
 
    // Compute error
    Scalar err = param_.cycle ? CycleValue<Scalar>(sp) - fb : sp - fb;
    Scalar k_err = err * param_.k;
 
    // Use externally provided derivative
    Scalar d = fb_dot;
    if (!std::isfinite(d))
    {
      d = 0;
    }
 
    // Compute PD
    Scalar output = (k_err * param_.p) - (d * param_.d);
 
    // Integrate if within limits
    Scalar i_term = i_ + k_err * dt;
    Scalar i_out = i_term * param_.i;
 
    if (param_.i > PID_SIGMA && std::isfinite(i_term))
    {
      if (std::abs(output + i_out) <= param_.out_limit &&
          std::abs(i_term) <= param_.i_limit)
      {
        i_ = i_term;
      }
    }
 
    // Apply output limits
    output += i_out;
    output += feed_forward_;
    if (std::isfinite(output) && param_.out_limit > PID_SIGMA)
    {
      output = std::clamp(output, -param_.out_limit, param_.out_limit);
    }
 
    // Store states
    last_err_ = err;
    last_fb_ = fb;
    last_out_ = output;
    last_der_ = d;
    return output;
  }
 
  void SetK(Scalar k) { param_.k = k; }
  void SetP(Scalar p) { param_.p = p; }
  void SetI(Scalar i) { param_.i = i; }
  void SetD(Scalar d) { param_.d = d; }
  void SetILimit(Scalar limit) { param_.i_limit = limit; }
  void SetOutLimit(Scalar limit) { param_.out_limit = limit; }
 
  Scalar K() const { return param_.k; }
  Scalar P() const { return param_.p; }
  Scalar I() const { return param_.i; }
  Scalar D() const { return param_.d; }
  Scalar ILimit() const { return param_.i_limit; }
  Scalar OutLimit() const { return param_.out_limit; }
  Scalar LastError() const { return last_err_; }
  Scalar LastFeedback() const { return last_fb_; }
  Scalar LastOutput() const { return last_out_; }
  Scalar LastDerivative() const { return last_der_; }
 
  void Reset()
  {
    i_ = 0;
    last_err_ = 0;
    last_fb_ = 0;
    last_out_ = 0;
    last_der_ = 0;
  }
 
  void SetIntegralError(Scalar err) { i_ = err; }
 
  Scalar GetIntegralError() const { return i_; }
 
  void SetFeedForward(Scalar feed_forward) { feed_forward_ = feed_forward; }
 
  Scalar GetFeedForward() const { return feed_forward_; }
 
 private:
  Param param_;              
  Scalar i_ = 0;             
  Scalar last_err_ = 0;      
  Scalar last_fb_ = 0;       
  Scalar last_der_ = 0;      
  Scalar last_out_ = 0;      
  Scalar feed_forward_ = 0;  
};
 
}  // namespace LibXR