icetek-pid.c

来自「2812例程以及 控制直流无刷电机程序」· C语言 代码 · 共 51 行

/*=====================================================================================
 File name:        PID_REG3.C  (IQ version)                  
                    
 Originator:	Digital Control Systems Group
			Texas Instruments

 Description:  The PID controller with anti-windup                   

=====================================================================================
 History:
-------------------------------------------------------------------------------------
 04-15-2005	Version 3.20
-------------------------------------------------------------------------------------*/

#include "IQmathLib.h"         // Include header for IQmath library
// Don't forget to set a proper GLOBAL_Q in "IQmathLib.h" file
#include "dmctype.h"
#include "ICETEK-PID.h"

void ICETEK_PIDA_calc(ICETEK_PIDA *v)
{	
    // Compute the error
    v->Err0 = v->Ref - v->Fdb;

    // Compute the proportional output
	v->Up = _IQmpy(v->Kp,v->Err0);

    // Compute the integral output
	v->Ui = _IQmpy(v->Ki,v->Err1);

    // Compute the derivative output
	v->Ud = _IQmpy(v->Kd,v->Err2);

    // Compute the pre-saturated output
    v->OutPreSat = v->Up + v->Ui + v->Ud;

    // Saturate the output
    if (v->OutPreSat > v->OutMax)
      v->Out =  v->OutMax;
    else if (v->OutPreSat < v->OutMin)
      v->Out =  v->OutMin;
    else
      v->Out = v->OutPreSat;

	v->Err2 = v->Err1; v->Err1 = v->Err0;

    // Compute the saturate difference
}