pid.cpp

Go to the documentation of this file.

 
#include <iostream>
#include <cmath>
#include "pid.h"
 
using namespace std;
 
class PIDImpl
{
    public:
        PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki );
        ~PIDImpl();
        void setIntegratorLimits(double min, double max)
        {
            m_IntegratorMin = min;
            m_IntegratorMax = max;
        }
        void setTau(double value)
        {
            m_Tau = value;
        }
        double calculate( double setpoint, double measurement );
        double propotionalTerm() const
        {
            return m_PropotionalTerm;
        }
        double integralTerm() const
        {
            return m_IntegralTerm;
        }
        double derivativeTerm() const
        {
            return m_DerivativeTerm;
        }
 
    private:
        // Sample Time
        double m_T {1};
        // Derivative Low-Pass filter time constant */
        double m_Tau {2};
 
        // Output limits
        double m_Max {0};
        double m_Min {0};
 
        // Integrator Limits
        double m_IntegratorMin {0};
        double m_IntegratorMax {0};
 
        // Gains
        double m_Kp {0};
        double m_Kd {0};
        double m_Ki {0};
 
        // Controller volatile data
        double m_PreviousError {0};
        double m_PreviousMeasurement {0};
 
        // Terms
        double m_PropotionalTerm {0};
        double m_IntegralTerm {0};
        double m_DerivativeTerm {0};
 
};
 
 
PID::PID( double dt, double max, double min, double Kp, double Kd, double Ki )
{
    pimpl = new PIDImpl(dt, max, min, Kp, Kd, Ki);
}
void PID::setIntegratorLimits(double min, double max)
{
    pimpl->setIntegratorLimits(min, max);
}
void PID::setTau(double value)
{
    pimpl->setTau(value);
}
double PID::calculate( double setpoint, double pv )
{
    return pimpl->calculate(setpoint, pv);
}
double PID::propotionalTerm() const
{
    return pimpl->propotionalTerm();
}
double PID::integralTerm() const
{
    return pimpl->integralTerm();
}
double PID::derivativeTerm() const
{
    return pimpl->derivativeTerm();
}
PID::~PID()
{
    delete pimpl;
}
 
 
PIDImpl::PIDImpl( double dt, double max, double min, double Kp, double Kd, double Ki ) :
    m_T(dt),
    m_Max(max),
    m_Min(min),
    m_Kp(Kp),
    m_Kd(Kd),
    m_Ki(Ki)
{
}
 
double PIDImpl::calculate(double setpoint, double measurement )
{
    // Calculate error
    double error = setpoint - measurement;
 
    // Proportional term
    m_PropotionalTerm = m_Kp * error;
 
    // Integral term
    m_IntegralTerm = m_IntegralTerm + 0.5 * m_Ki * m_T * (error + m_PreviousError);
 
    // Clamp Integral
    if (m_IntegratorMin || m_IntegratorMax)
        m_IntegralTerm = std::min(m_IntegratorMax, std::max(m_IntegratorMin, m_IntegralTerm));
 
    // Derivative term (N.B. on measurement NOT error)
    m_DerivativeTerm = -(2.0f * m_Kd * (measurement - m_PreviousMeasurement) + (2.0f * m_Tau - m_T) * m_DerivativeTerm)
                       / (2.0f * m_Tau + m_T);
 
    // Calculate total output
    double output = m_PropotionalTerm + m_IntegralTerm + m_DerivativeTerm;
 
    // Clamp Output
    output = std::min(m_Max, std::max(m_Min, output));
 
    // Save error to previous error
    m_PreviousError = error;
    m_PreviousMeasurement = measurement;
 
    return output;
}
 
PIDImpl::~PIDImpl()
{
}