File content as of revision 1:09a162f4f6ce:

#include "mbed.h"
#include "pid.h"


PID::PID(void){
    initialize();
}

PID::PID(double Pgain, double Igain, double Dgain){
    initialize();
    setPIDgain(Pgain,Igain,Dgain);
}

PID::PID(double Pgain, double Igain, double Dgain, double Max, double Min){
    initialize();
    setPIDgain(Pgain,Igain,Dgain);
    setMaxMin(Max,Min);
}

PID::~PID(){

}

void PID::initialize(void){
    kp = 0.0;   ki = 0.0;   kd = 0.0;
    max = 0.0; min = 0.0;
    dt = 0.0;
    integral = 0.0;
    for(uint8_t i=0; i<2; i++){
        oldval[i] = 0.0;
        diff[i] = 0.0;
    }
    maxcheck = false;
    mincheck = false;
}

void PID::setPIDgain(double Pgain, double Igain, double Dgain){
    kp = Pgain;
    ki = Igain;
    kd = Dgain;
}

void PID::setMaxMin(double Max, double Min){
    if(Max < Min) return;   //最大値<最小値であれば設定せずに終了
    max = Max;
    min = Min;
    maxcheck = true; mincheck = true;
}

void PID::switchMaxMin(bool Maxcheck, bool Mincheck){
    maxcheck = Maxcheck;
    mincheck = Mincheck;
}

double PID::calcPID(double nowval, double targetval, double dt){
    double p,i,d,pid;

    diff[1] = diff[0];
    diff[0] = nowval - targetval;
    if(diff[1] == 0.0) return 0.0;   //前回の値がない場合，0.0を返す

    integral += (diff[0] + diff[1]) / 2.0 * dt;

    p = kp * diff[0];
    i = ki * integral;
    d = kd * (diff[0] - diff[1]) / dt;
    pid = p + i + d;
    if(maxcheck && pid>max) pid = max;
    if(mincheck && pid<min) pid = min;

    return pid;

}