File content as of revision 0:1f4d5c5491b8:

#include "PID.h"

PIDClass::PIDClass(int _Kr,int _Ki, int _Kd,int _SetPoint):Kr(_Kr),Ki(_Ki),Kd(_Kd),SetPoint(_SetPoint)
{
    /*Kr = _Kr;
    Ki = _Ki;
    Kd = _Kd;*/
}


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;
    
}