File content as of revision 0:0d02a451e79d:

#include "pid.h"

void PositionPid::setup(float Kp, float Ki, float Kd, float dt){
    kp = Kp;
    ki = Ki;
    kd = Kd;
    time = dt;
    frequency = 1/dt;
}

void PositionPid::calculate(float target, float nowValue){
    old = now;
    now = nowValue - target;
    p = now;
    i = i + (now + old) * 0.5f * time;
    d = (now - old) * frequency;
    result = kp*p + ki*i + kd*d;
    if (result > 1.0f) {
        result = 1.0f;
    } else if (result < -1.0f) {
        result = -1.0f;
    }
}

float PositionPid::duty(){
    return result;
}

void SpeedPid::setup(float Kp, float Ki, float Kd, float dt){
    kp = Kp;
    ki = Ki;
    kd = Kd;
    time = dt;
    frequency = 1/dt;
}

void SpeedPid::calculate(float target, float nowValue){
    e2 = e1;
    e1 = e;
    e = nowValue - target;
    p = e - e1;
    i = e*time;
    d = (e-2*e1+e2)*frequency;
    result = result + (kp*p+ki*i+kd*d);
    if (result > 1.0f) {
        result = 1.0f;
    } else if (result < -1.0f) {
        result = -1.0f;
    }
}

float SpeedPid::duty(){
    return result;
}