PID(三、增量型PID)

#include <stdio.h>

typedef struct{
    float SetLocation;
    float LastLocation;
    float err_now_0;
    float err_next_1;
    float err_last_2;
    float Kp;
    float Ki;
    float Kd;
    float output;
}PID;

PID pid;

void
PID_Config(PID *pid)
{
    (*pid).SetLocation = 0.0;
    (*pid).LastLocation = 0.0;
    (*pid).err_now_0 = 0.0;
    (*pid).err_next_1 = 0.0;
    (*pid).err_last_2 = 0.0;
    (*pid).Kp = 0.2;
    (*pid).Ki = 0.015;
    (*pid).Kd = 0.2;
    (*pid).output = 0.0;
}

float
PID_SetLocation(PID *pid, float location)
{
    float  IncrementLocation;

    (*pid).SetLocation = location;
    (*pid).err_now_0 = (*pid).SetLocation- (*pid).LastLocation;
    IncrementLocation = (*pid).Kp * ((*pid).err_now_0 - (*pid).err_next_1) + (*pid).Ki * (*pid).err_now_0 + (*pid).Kd * ((*pid).err_now_0 - 2 * (*pid).err_next_1 + (*pid).err_last_2);
    /*△Uo(n) = P * [e(n)-e(n-1)] + I * e(n)+ D * [e(n)-2e(n-1)+e(n-2)]*/
    (*pid).output = (*pid).LastLocation +IncrementLocation;
    (*pid).err_last_2 = (*pid).err_next_1;
    (*pid).err_next_1 = (*pid).err_now_0;
    (*pid).LastLocation = (*pid).output;

    return   (*pid).output;
}

int
main(int argc, char *argv[])
{
    int count = 0;
    float location;

    PID_Config(&pid);

    while (count < 10000) {
        location = PID_SetLocation(&pid, 200.0);
        count++;

        if (location > 199.99) {
            break;
        }
    }

    printf("program has execute %d time \n", count);

    return 0;
}

#if 0
program has execute 773 time

Process returned 0 (0x0)   execution time : 0.034 s
#endif // 0