pid_reg3.c

来自「TI公司的DSP(LF2407A)编写的有霍尔位置传感器的无刷直流电机控制程序,」· C语言 代码 · 共 48 行

/*=====================================================================================
 File name:        PID_REG3.C   
 Description:  The PID controller with anti-windup                   

 05-31-2007	Version 3.20
-------------------------------------------------------------------------------------*/

#include <pid_reg3.h>

void pid_reg3_calc(PIDREG3 *v)
{	
    
    long UpTemp,UiTemp;
    // Compute the error
    v->Err = v->Ref - v->Fdb;

    // Compute the proportional output
	UpTemp = (long)(v->Kp*v->Err);
    v->Up = (int)(UpTemp>>9);

    // Compute the integral output
	UiTemp = (long)(v->Ki*v->Up);
    v->Ui = v->Ui + (int)(UiTemp>>4) + (int)((long)((v->Kc)*(v->SatErr))>>13);

    // Compute the derivative output
    v->Ud = (int)((long)((v->Kd)*(v->Up - v->Up1))>>13);

    // 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;

    // Compute the saturate difference
    v->SatErr = v->Out - v->OutPreSat;

    // Update the previous proportional output
    v->Up1 = v->Up; 

}