File content as of revision 5:6949e401a9ad:

#include "PID.h"

PID::PID(Timer *T)
{
    timer = T;
    integral    = 0;
    prev_hensa  = 0;
    nowtime     = 0;
    prev_time   = 0;
    lateD      = 0;
    timer->start();
}
float PID::controlPID(float target, float nowrpm)
{
    nowtime = timer->read();
    float hensa = target - nowrpm;
    float dt = nowtime - prev_time;
    integral += (hensa + prev_hensa) / 2 * dt;
    float differential = (hensa - prev_hensa) / dt;
    float sousaryou = Kp*hensa + Ki*integral + Kd*differential;
    //printf("%f %f %f\r\n", Kp*hensa, Ki*integral, Kd*differential);
    prev_hensa = hensa;
    prev_time = nowtime;
    return sousaryou;
}
float PID::PI_lateD(float target, float nowrpm)
{
    nowtime = timer->read();
    float hensa = target - nowrpm;
    float dt = nowtime - prev_time;
    integral += (hensa + prev_hensa) / 2 * dt;
    float sousaryou = Kp*hensa + Ki*integral + Kd*lateD;
    prev_hensa = hensa;
    prev_time = timer->read();
    lateD = (hensa - prev_hensa) / dt;
    return sousaryou;
}
float PID::controlP(float target, float nowrpm, float new_Kp)
{
    float hensa = target - nowrpm;
    float sousaryou = new_Kp*hensa;
    return sousaryou;
}
float PID::controlPI(float target, float nowrpm)
{
    nowtime = timer->read();
    float hensa = target - nowrpm;
    float dt = nowtime - prev_time;
    integral += (hensa + prev_hensa) / 2 * dt;
    float sousaryou = Kp*hensa + Ki*integral;
    //printf("%f %f %f\r\n", Kp*hensa, Ki*integral, Kd*differential);
    prev_hensa = hensa;
    prev_time = timer->read();
    return sousaryou;
}
void PID::setParameter(float new_Kp, float new_Ki, float new_Kd)
{
    Kp = new_Kp;
    Ki = new_Ki;
    Kd = new_Kd;
}
void PID::setParameter(float new_Ku, float new_Pu)
{  
    Ku = new_Ku;
    Pu = new_Pu;
    
    Kp = 0.60 * Ku;
    Ti = 0.50 * Pu;
    Td = 0.125 * Pu;
    Ki = (1 / Ti) * Kp;
    Kd = Td * Kp;
}
void PID::setParameterPI(float new_Ku, float new_Pu)
{
    Kp = 0.45 * Ku;
    Ti = 0.83 * Pu;
    Ki = (1 / Ti) * Kp;
}
void PID::reset()
{
    integral    = 0;
    prev_hensa  = 0;
    prev_time = timer->read();
}