File content as of revision 2:0413525bbb2e:

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

PID::PID(float p, float i, float d) {
    _p = p;
    _i = i;
    _d = d;

    target_val = 0.0;
    integral_val = 0.0;
    last_time = 0; //us
    last_val = 0.0;

    timer.start();
};

float PID::get_input(float val) {
    int now_time = timer.read_us();
    float dt = now_time - last_time;
    dt /= pow(10.0, 6.0);

    val = target_val - val;

//// normal PID
//    integral_val += (last_val * val) / 2 * dt;
//    float d_val = (val - last_val) / dt;
//    last_time = now_time;
//    last_val = val;
//    return _p * val+ _i * integral_val + _d * d_val;

//// original
    if(abs(integral_val) < 50 || integral_val * val < 0) {
        integral_val += val * _p;
    }
    return integral_val;
};

void PID::set_target(float val) {
    target_val = val;
    reset_integral_val();
};

void PID::reset_integral_val() {
    integral_val = 0.0;
};