PID(六、梯形積分PID)

#include <stdio.h>
#include <math.h>
#include <stdlib.h>

typedef struct {
    float SetSpeed;
    float LastLocation;
    float err_now_0;
    float err_last_1;
    float Kp;
    float Ki;
    float Kd;
    float output;
    float intergal_sum;
    float umax;
    float umin;
}PID;

PID pid;

void
PID_Config(PID *pid)
{
    (*pid).SetSpeed = 0.0;
    (*pid).LastLocation = 0.0;
    (*pid).err_now_0 = 0.0;
    (*pid).err_last_1 = 0.0;
    (*pid).output = 0.0;
    (*pid).intergal_sum = 0.0;
    (*pid).Kp = 0.2;
    (*pid).Ki = 0.1;
    (*pid).Kd = 0.2;
    (*pid).umax = 400;
    (*pid).umin = -200;
}
float
PID_SetSpeed(PID *pid, float speed)
{
    char index;

    (*pid).SetSpeed = speed;
    (*pid).err_now_0 = (*pid).SetSpeed-(*pid).LastLocation;

    if ((*pid).LastLocation > (*pid).umax) {  /*have arrived the max speed*/
        if (abs((*pid).err_now_0) > 200) {
            index = 0;
        }else{
            index = 1;

            if ((*pid).err_now_0 < 0){
                (*pid).intergal_sum += (*pid).err_now_0;
            }
        }
    } else if ((*pid).LastLocation < (*pid).umin){ /*have arrived the min speed*/
        if (abs((*pid).err_now_0) > 200) {
            index = 0;
        }else{
            index = 1;
            if ((*pid).err_now_0 > 0) {
                (*pid).intergal_sum += (*pid).err_now_0;
            }
        }
    } else{                                 /*not arrived  the max/min speed*/
        if (abs((*pid).err_now_0 > 200)){
            index = 0;
        } else {
            index = 1;
            (*pid).intergal_sum += (*pid).err_now_0;
        }
    }

    (*pid).output = (*pid).Kp * (*pid).err_now_0 + index * (*pid).Ki * (*pid).intergal_sum / 2+ (*pid).Kd * ((*pid).err_now_0-(*pid).err_last_1);
    (*pid).err_last_1 = (*pid).err_now_0;
    (*pid).LastLocation = (*pid).output*1.0;

    return  (*pid).LastLocation;
}

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

    PID_Config(&pid);
    while (count < 10000) {
        speed = PID_SetSpeed(&pid, 200.0);
        count++;

        if (speed > 199.99) {
            break;
        }
    }

    printf("program have exe %d times ", count);

    return 0;
}