File content as of revision 2:7fede27af6ca:

#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,bool enableErrorIntegration) {
    switch(MODE) {
        case 0:
            before = now;
            now = targetValue - nowValue;
            p = now;
            if(enableErrorIntegration) i += (now + before) * PERIOD / 2;           //積分
            else                       i = 0.0f; 
            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 > 1.0f) {
        duty = 1.0f;
    } else if (duty < -1.0f) {
        duty = -1.0f;
    }
};

void Pid::reset() {
    duty = 0.0f;   
}

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