File content as of revision 2:c8ab3e8d4c51:

#include "PID.h"
#include "mbed.h"

PID::PID(double* _Input, double* _Output, double* _Setpoint, double _Kp, double _Ki, double _Kd, PID_Mode _mode, Timer *_timer)
{

    Input = _Input;
    Output = _Output;
    Setpoint = _Setpoint;

    timer = _timer;
    
    SampleTime = 100;
    lastTime = timer->read_ms();
    setOutputLimits(-1,1);
    setParameter_pid(_Kp,_Ki,_Kd);
    setPIDMode(_mode);
    Initialize();
}

void PID::control()
{
    //timer->start();
    double nowtime =timer->read_ms();
    double input = *Input;
    
    if((nowtime - lastTime) > SampleTime)
    {    
        double error = *Setpoint - input;
        double dInput = input - lastInput;
        double derror = error - lastError;
        outputSum += (Ki*error);
        double output = 0;
        
        if(outputSum > outMax) outputSum= outMax;
        else if(outputSum < outMin) outputSum= outMin;
        
        switch(mode)
        {
            case pos_PID:
                output = outputSum + Kp*error + Kd*derror;
                break; 
                
            case P_D:
                output = Kp*error - Kd*dInput;
                break;
            
            case PI_D:
                output = outputSum + Kp*error - Kd*dInput;
                break;
            
            case I_PD:
                output = outputSum - Kp*input - Kd*dInput;
                break;
                
        }
        
        *Output = output;
        lastTime = nowtime;
        lastInput = input;
    }
}

void PID::setParameter_pid(double new_Kp, double new_Ki, double new_Kd)
{
    Kp = new_Kp;
    Ki = new_Ki;
    Kd = new_Kd;
}

void PID::setParameter_KuPu(double new_Ku, double new_Pu)
{
    Ku = new_Ku;
    Pu = new_Pu;
    Kp = 0.60 * Ku;
    Ti = 0.50 * Pu;
    Td = 0.125 * Pu;
    Ki = (1 / Ti) * Kp;
    Kd = Td * Kp;
}

void PID::setPIDMode(PID_Mode _mode)
{
    mode = _mode;
    Initialize();
}

void PID::setOutputLimits(double Min, double Max)
{
    if(Min >= Max) return;
    outMin = Min;
    outMax = Max;

    if(*Output > outMax) *Output = outMax;
    else if(*Output < outMin) *Output = outMin;
    if(outputSum > outMax) outputSum = outMax;
    else if(outputSum < outMin) outputSum = outMin;
}

void PID::reset(double target)
{
    outputSum = 0;
}

void PID::Initialize()
{
    outputSum = *Output;
    lastInput = *Input;
    lastError = *Setpoint - lastInput;
    if(outputSum > outMax) outputSum = outMax;
    else if(outputSum < outMin) outputSum = outMin;
}