File content as of revision 0:e83b840a5f86:

#include "PID_Control.h"

PID_Control::PID_Control(double kp, double ki, double kd)
{
    set_PID(kp, ki, kd);
    integral = 0.0;
}

double PID_Control::PID(double present, double target, double interval)
{
    diff[0] = diff[1];
    diff[1] = target - present;
    integral += ((diff[0] + diff[1]) / 2.0) * interval;
    p = kp * diff[1];
    i = ki * integral;
    d = kd * (diff[1] - diff[0]);
    control = p + i + d;
    //printf("p[%f] i[%f] d[%f] ", p, i, d);
    //printf("p:%.3f\t:%.3f\tc:%.3f\t:%.3f\r\n", present, target, control, interval);
    //printf("t:%.3f\r\n", target);
    return control;
}

void PID_Control::reset()
{
    diff[0] = 0;
    diff[1] = 0.0;
    integral = 0.0;
    control = 0;
}

void PID_Control::set_PID(double kp, double ki, double kd)
{
    this->kp = kp;
    this->ki = ki;
    this->kd = kd;
    reset();
}