File content as of revision 0:76fdba038ca7:

#include "mbed.h"
#include "CalPIDF.h"

CalPIDF::CalPIDF(double kp_, double ki_, double kd_,double dt, double max)
{
    setParameter(kp_, ki_, kd_);
    deviation_old = 0;
    integral = 0;
    delta_t=dt;
    setMaxValue(max);
};
void CalPIDF::setParameter(double kp_, double ki_, double kd_)
{
    kp = kp_;
    ki = ki_;
    kd = kd_;
}
void CalPIDF::setMaxValue(double max)
{
    max_pid = max;
}
void CalPIDF::setDELTA_T(double delta_time)
{
    delta_t = delta_time;
}
double CalPIDF::calPID(double devia_present)
{
    double differential = (deviation_old - devia_present) / delta_t;
    if(value_PID!=max_pid&&value_PID!=-max_pid) {//アンチワインドアップ（積分項固定式）
        integral += (devia_present + deviation_old) * delta_t / 2;
    }
    value_PID = kp * devia_present + ki * integral - kd * differential;

    if (value_PID > max_pid) {
        value_PID = max_pid;
    } else if (value_PID < -max_pid) {
        value_PID = -max_pid;
    }

    deviation_old = devia_present;

    return value_PID;
}
double CalPIDF::calPD(double devia_present)
{
    double differential = (deviation_old - devia_present) / delta_t;
    double value_PD = kp * devia_present - kd * differential;

    if (value_PD > max_pid) {
        value_PD = max_pid;
    } else if (value_PD < -max_pid) {
        value_PD = -max_pid;
    }

    deviation_old = devia_present;

    return value_PD;
}
double CalPIDF::calP_D(double devia_present,double diff_value)
{
    double value_P_D = kp * devia_present - kd * diff_value;

    if (value_P_D > max_pid) {
        value_P_D = max_pid;
    } else if (value_P_D < -max_pid) {
        value_P_D = -max_pid;
    }

    return value_P_D;
}
void CalPIDF::resetIntegral()
{
    integral = 0;
}