File content as of revision 0:630126b8994f:

#include "Pid.h"

void Pid::setup(float Kp,float Ki,float Kd,short PidMode,float period) {
    KP = Kp;
    KI = Ki;
    KD = Kd;
    MODE = PidMode;
    PERIOD = period;
    //pid.attach(this,&Pid::calculate,period);
}

void Pid::calculate(float targetValue,float nowValue) {
    switch(MODE) {
        case 0:
            before = now;
            now = targetValue - nowValue;
            p = now;
            i += (now + before) * PERIOD / 2;           //積分
            d = (now - before) / PERIOD;                //微分
            //target_duty = p * KP + i * KI + d * KD;
            duty = p * KP + i * KI + d * KD;
            break;
        case 1:
            e2 = e1;
            e1 = e;
            e = targetValue - nowValue;
            //p = (e - e1) / PERIOD;
            p = e / PERIOD;
            i = e;
            d = (e - 2*e1 + e2)/PERIOD;
            duty += p * KP + i * KI + d * KD;           //dutyは微分されたものなので、積分してあげる(もとに戻す)
            break;
        default:
            break;
    }
    
    if(duty > 0.95f) {
        duty = 0.95f;
    } else if (duty < -0.95f) {
        duty = -0.95f;
    }
};

float Pid::getDuty() {
    return duty;
}