File content as of revision 4:3ca1603fbcda:

#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::control(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 = timer.read();
    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::control_P(float target, float nowrpm, float new_Kp)
{
    float hensa = target - nowrpm;
    float sousaryou = new_Kp*hensa;
    return sousaryou;
}
float PID::control_PI(float target, float nowrpm)
{
    Kp = 0.45 * Ku;
    Ti = 0.83 * Pu;
    Ki = (1 / Ti) * Kp;
    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::set_parameter(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::reset()
{
    integral    = 0;
    prev_hensa  = 0;
    prev_time = timer.read();
}