File content as of revision 1:d3e12393b71d:

#include "PID.h"

PIDClass::PIDClass(float _Kr,float _Ki, float _Kd,float _SetPoint):Kr(_Kr),Ki(_Ki),Kd(_Kd),SetPoint(_SetPoint)
{
    //
}

float PIDClass::ComputeCommand(float inputADC)
{
    static PID_static param;
    static float inter = Ki*Kd;
    static float Kc = sqrt(inter);
    float outputPWM = 0;
    float error = 0;
    
    error = SetPoint - inputADC;
    param.integral = param.integral + Ki*Te*error;
    outputPWM = Kr*error + param.integral + Kd*(error - param.last_error)/Te;
    param.last_error = error;
    
    if(outputPWM > SUPERIOR_MARGIN)
    {
        param.integral = param.integral + Kc*(SUPERIOR_MARGIN - outputPWM);
        outputPWM = SUPERIOR_MARGIN;
    }else if(outputPWM < INFERIOR_MARGIN)
    {
        param.integral = param.integral + Kc*(INFERIOR_MARGIN - outputPWM);
        outputPWM = 0;   
    }
    
    return outputPWM;
    
}