pid_reg2.c

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

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

 06-9-2007	Version 4.0
-------------------------------------------------------------------------------------*/

#include <PID2.h>

void pid2_calc(PID2 *v)
{	
    int Up;
    long UpTemp,UiTemp;
    // Compute the error
    v->en0_reg2 = v->ref_reg2 - v->fb_reg2;

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

    // Compute the integral output
	UiTemp = ((long)(v->k1_reg2))*UpTemp;
	UiTemp = UiTemp>>13;
    v->un_reg2 = v->un_reg2 + (int)UiTemp + (int)(((long)(v->kc_reg2)*(long)(v->upi_reg2))>>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;
	v->epi_reg2 = Up + v->un_reg2;
    // Saturate the output
    if (v->epi_reg2 > v->max_reg2)
      v->out_reg2 =  v->max_reg2;
    else if (v->epi_reg2 < v->min_reg2)
      v->out_reg2 =  v->min_reg2;
    else
      v->out_reg2 = v->epi_reg2;

    // Compute the saturate difference
    v->upi_reg2 = v->out_reg2 - v->epi_reg2;

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

}