PID模板

#define  PID_C
#include "pid.h"

void PID_Calculate(PID_TypeDef *Data)
{
    float Inc, P, I, D, Err;
    
    Err = Data->Target - Data->Current;
    
    Data->Error[0] = Data->Error[1];
    Data->Error[1] = Data->Error[2];
    Data->Error[2] = (Abs_float(Err) > Data->DeadZone) * Err;
    
    P = Data->Kp*(Data->Error[2]-Data->Error[1]);
    I = Data->Ki*(Data->Error[2]);
    D = Data->Kd*(Data->Error[2]-2*Data->Error[1]+Data->Error[0]);
    Inc = (P+I+D);
    
    Data->Output += Inc;
    Data->Output = Min_float(Data->Output, Data->OutputLimit);
    Data->Output = Max_float(Data->Output,-Data->OutputLimit);
}

void PID_Clear(PID_TypeDef *Data)
{
    Data->Current   = 0;
    
    Data->Error[0]  = 0;
    Data->Error[1]  = 0;
    Data->Error[2]  = 0;
    
    Data->Output    = 0;
}

#ifndef  PID_H
#define  PID_H
#include "main.h"

#ifdef   PID_C

#endif

typedef struct
{
    float Kp,Ki,Kd;
    float Target;
    float Current;
    float Error[3];
    float DeadZone;float Output;
    float OutputLimit;
} PID_TypeDef;

void PID_Calculate(PID_TypeDef *Data);
void PID_Clear    (PID_TypeDef *Data);

#endif