File content as of revision 4:369caebdf5dc:

#include "PID.h"

void
PID::setP(float p)
{
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    k_p = p;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
}

void
PID::setI(float i)
{
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    k_i = i;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
}

void 
PID::setD(float d)
{
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    k_d = d;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
}

void 
PID::setClip(float clip)
{
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    this->clip = clip;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ
}

void
PID::setPV(float pv) // Set process variable
{
    NVIC_DisableIRQ(TIMER3_IRQn);   // Disable Ticker IRQ for atomicity
    error = pv;
    NVIC_EnableIRQ(TIMER3_IRQn);    // Enable Ticker IRQ

}

void
PID::dumpDebug(Serial *debug)
{
    if (debug != NULL)
    {
        debug->printf("PID Debug: v_p        = %3.2f\r\n", v_p);
        debug->printf("PID Debug: v_i        = %3.2f\r\n", v_i);
        debug->printf("PID Debug: v_d        = %3.2f\r\n", v_d);
        debug->printf("PID Debug: k_p        = %3.2f\r\n", k_p);
        debug->printf("PID Debug: k_i        = %3.2f\r\n", k_i);
        debug->printf("PID Debug: k_d        = %3.2f\r\n", k_d);
        debug->printf("PID Debug: clip       = %3.2f\r\n", clip);
        debug->printf("PID Debug: error      = %3.2f\r\n", error);
        debug->printf("PID Debug: prev_error = %3.2f\r\n", prev_error);
        debug->printf("PID Debug: output     = %3.2f\r\n", output);
    }
}

float
PID::run(float in)
{
    if (debug != NULL)
        debug->printf("PID Debug: in         = %3.2f\r\n", in);
    error = in;
    
    v_p  = error;
    
    if (k_i != 0)
        v_i += v_p;
    else
        v_i = 0;
        
    v_d  = error - prev_error;    
    
    if (v_i > clip)
        v_i = clip;
    if (v_i < -clip)
        v_i = -clip;
        
    if (v_i != v_i) // NAN check...
        v_i = 0;
        
    if (debug != NULL)
    {
        debug->printf("PID Debug: v_p        = %3.2f\r\n", v_p);
        debug->printf("PID Debug: v_i        = %3.2f\r\n", v_i);
        debug->printf("PID Debug: v_d        = %3.2f\r\n", v_d);
        debug->printf("PID Debug: k_p        = %3.2f\r\n", k_p);
        debug->printf("PID Debug: k_i        = %3.2f\r\n", k_i);
        debug->printf("PID Debug: k_d        = %3.2f\r\n", k_d);
        debug->printf("PID Debug: clip       = %3.2f\r\n", clip);
    }
    
    output = (v_p * k_p) + (v_i * k_i) + (v_d * k_d);
    
    if (debug != NULL)
    {
        debug->printf("PID Debug: error      = %3.2f\r\n", error);
        debug->printf("PID Debug: prev_error = %3.2f\r\n", prev_error);
        debug->printf("PID Debug: output     = %3.2f\r\n", output);
    }
    
    prev_error = error;
    
    return output;
}