File content as of revision 37:8719223998d8:

#include "pidcontrol.h"

PID::PID()
{
    Kp=1.0f;
    Ki=0.0f;
    Kd=0.0f;
    il=65535.0;
    margin = 0.0f;

}

PID::PID(float p,float i,float d)
{
    Kp=p;
    Ki=i;
    Kd=d;
    il=65535.0;
    margin =0.0f;
}

void PID::setGoal(float ref)
{
    setpoint = ref;
}

void PID::setCurrent(float sensor)
{
    input = sensor;
}

float PID::compute()
{

    e_n = setpoint - input;

    if((e_i < il) && (e_i > -il))
    {
        e_i += e_n;
    }
    else
    {
#ifdef PID_DEBUG
        printf("il overflow\n\r");
#endif
        e_i =il;
    }


    output = (Kp*e_n)+(Ki*e_i)+(Kd*(e_n-e_n_1));

    if(output > 0)
    {
        if(output < margin)
        {
            output = 0.0;
        }
    }
    else
    {
        if(output > -margin)
        {
            output = 0.0;
        }
    }

    return output;
}

void PID::setMargin(float gap)
{
    margin =gap;
}

float PID::getMargin()
{
    return margin;
}


void PID::setIntegalLimit(float limit)
{
    il = limit;
}
float PID::getIntegalLimit()
{
    return il;
}


float PID::getErrorNow()
{
    return e_n;
}

float PID::getErrorLast()
{
    return e_n_1;
}

float PID::getErrorDiff()
{
    return e_n - e_n_1;
}

float PID::getErrorIntegal()
{
    return e_i;
}

void PID::setKp(float p)
{
    if(p < 0.0f)
    {
#ifdef PID_DEBUG
        printf("Kp = 0.0\n\r");
#endif
        Kp=0.0;
    }
    else
    {
        Kp=p;
    }
}

void PID::setKi(float i)
{
    if(i < 0.0f)
    {
#ifdef PID_DEBUG
        printf("Ki = 0.0\n\r");
#endif
        Ki=0.0;
    }
    else
    {
        Ki=i;
    }
}
void PID::setKd(float d)
{
    if(d < 0.0f)
    {
#ifdef PID_DEBUG
        printf("Kd = 0.0\n\r");
#endif
        Kd=0.0;
    }
    else
    {
        Kd=d;
    }
}

float PID::getKp()
{
    return Kp;    
}

float PID::getKi()
{   
    return Ki;    
}

float PID::getKd()
{   
    return Kd;    
}