PID.h

#pragma once
#include <Arduino.h>
#include <limits.h>
 
namespace PID
{
    constexpr bool EXCEED_MICROS_LIMIT = false;
 
    constexpr unsigned long MAX_MICROSECONDS = ULONG_MAX;
    constexpr double MICROSECONDS_TO_SECONDS = 1.0 / 1000000.0;
 
    class PID
    {
    private:
        double Kp;
        double Ki;
        double Kd;
        double current;
        double target;
        double diff;
        double preDiff;
        double integral;
        unsigned long preMicros;
 
        double dutyCycle = 0;
        double capableDutyCycle;
 
        bool direction;
 
    public:
        double dt;
 
        PID(double capableDutyCycle, double Kp, double Ki, double Kd, double current, double target, bool direction);
 
        void setGains(double Kp, double Ki, double Kd);
 
        void setKp(double Kp);
 
        void setKi(double Ki);
 
        void setKd(double Kd);
 
        void setTarget(double target);
 
        double getTarget() const;
 
        double getCurrent() const;
 
        double getDutyCycle() const;
 
        void reset();
 
        void reset(double target);
 
        void reset(double Kp, double Ki, double Kd);
 
        void reset(double Kp, double Ki, double Kd, double target);
 
        double compute_PID(double current, bool logging = false);
 
        double getDt() const
        {
            return dt;
        }
    };
 
    inline void PID::setGains(const double Kp, const double Ki, const double Kd)
    {
        this->Kp = Kp;
        this->Ki = Ki;
        this->Kd = Kd;
    }
    inline void PID::setKp(const double Kp)
    {
        this->Kp = Kp;
    }
    inline void PID::setKi(const double Ki)
    {
        this->Ki = Ki;
    }
    inline void PID::setKd(const double Kd)
    {
        this->Kd = Kd;
    }
    inline void PID::setTarget(const double target)
    {
        this->target = target;
    }
    inline double PID::getTarget() const
    {
        return this->target;
    }
    inline double PID::getCurrent() const
    {
        return this->current;
    }
    inline double PID::getDutyCycle() const
    {
        return this->dutyCycle;
    }
    inline void PID::reset()
    {
        preMicros = micros();
        preDiff = 0;
        integral = 0;
    }
    inline void PID::reset(const double target)
    {
        this->target = target;
        this->reset();
    }
 
    inline void PID::reset(const double Kp, const double Ki, const double Kd)
    {
        this->Kp = Kp;
        this->Ki = Ki;
        this->Kd = Kd;
        this->reset();
    }
 
    inline void PID::reset(const double Kp, const double Ki, const double Kd, const double target)
    {
        this->Kp = Kp;
        this->Ki = Ki;
        this->Kd = Kd;
        this->target = target;
        this->reset();
    }
 
}