File content as of revision 0:9a6f7aafe531:

#include "PID.h"
/*
    Bryce Williams 11/19/2015
    See PID.h for references as well as method descriptions
*/

PID::PID(float* setpoint, float* feedback, float* output,
         float output_lower, float output_upper,
         float  kp, float ki,  float kd, float Ts){
    _Ts = Ts;           // Init params
    _kp = kp; 
    _ki = ki*Ts;        // Roll sample time into gain  
    _kd = kd / Ts;      // Roll sample time into gain
    
    _setpoint = setpoint; 
    _feedback = feedback; 
    _output = output;
    
    _output_lower = output_lower;
    _output_upper = output_upper;
}

void PID::start(){
    // Start up such we avoid bumps... (see "Initialization" section in
    // the reference link found in the header file).
    last_feedback = *_feedback;   // Eliminate derivative kick at start/restart
    i_accumulator = clip(*_output, _output_lower,
                     _output_upper);    // P and D terms are zero, thus 
                                        // i term is used to keep output unchanged
    
    sample_timer.attach(this, &PID::sample, _Ts);
}

void PID::stop(){
    sample_timer.detach();
}

float PID::getError(){
    return error;
}

void PID::set_parameters(float kp, float ki, float kd, float Ts){
    stop();         // Disable Sample Interrupt... stop()
    _Ts = Ts;       // Set New Sample Time
    _kp = kp;       // Seet New Kp
    _ki = ki*Ts;    // Roll sample time into gain  
    _kd = kd / Ts;  // Roll sample time into gain
    start();        // Enable Sample Interrupt... start()
}

void PID::sample(){
    error = *_setpoint - *_feedback;

    // Accumulate Integral Term such ki is applied to current error
    // before adding to pool; avoids bumps if ki gain value is changed.
    i_accumulator += _ki * error;
    // Avoid "Windup" by clamping intergral term to output limits;
    // essentially we stop integrating when we reach an upper or 
    // lower bound.
    i_accumulator = clip(i_accumulator, _output_lower, _output_upper);
    
    // Run it!
    *_output = _kp*error + i_accumulator - _kd*(*_feedback - last_feedback);
    last_feedback = *_feedback;
    // Clamp Output
    *_output = clip(*_output, _output_lower, _output_upper);
}

float PID::clip(float value, float lower, float upper){
    return std::max(lower, std::min(value, upper));
}