/*=====================================================================================
 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 "DSP281x_Device.h"
#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 "pid_reg3.h"

void pid_reg3_calc(PIDREG3 *v)
{	
    // Compute the error
    v->Err = v->Ref - v->Fdb;
    // Compute the proportional output
    v->Up = _IQmpy(v->Kp,v->Err-v->Err1);
    // Compute the integral output
    v->Ui =_IQmpy(v->Ki,v->Err);

    v->DeltaOut = v->Up + v->Ui;

#if Limit_Pid_Delta==1
    // Saturate the Delta Output
    if (v->DeltaOut > v->DeltaOutMax)
      v->DeltaOut =  v->DeltaOutMax;
    else if (v->DeltaOut < v->DeltaOutMin)
      v->DeltaOut =  v->DeltaOutMin;
    else
      v->DeltaOut =  v->DeltaOut;
#endif

    //求PID輸出
    v->Out=v->Out+v->DeltaOut;
    // Saturate the Output
    if (v->Out > v->OutMax)
      v->Out =  v->OutMax;
    else if (v->Out < v->OutMin)
      v->Out =  v->OutMin;
    else
      v->Out =  v->Out;
    //存儲本次誤差
      v->Err1=v->Err;

    

}

PIDREG3 PID_Voltage={      0, \
                           0, \
                           0, \
						   0, \
                           _IQ(5), \
                           0, \
						   _IQ(0.001), \
                           0, \
                           0, \
                           _IQ(0.5), \
                           _IQ(-0.5), \
                           0, \
						   _IQ(Ctl_Frequency-1), \
                           _IQ(0), \
              			  (void (*)(Uint32))pid_reg3_calc
              	      };


PIDREG3 PID_Current={      0, \
                           0, \
                           0, \
						   0, \
                           _IQ(5), \
                           0, \
						   _IQ(0.001), \
                           0, \
                           0, \
                           _IQ(1), \
                           _IQ(-1), \
                           0, \
						   _IQ(Ctl_Frequency-1), \
                           _IQ(0), \
              			  (void (*)(Uint32))pid_reg3_calc
              	      };

void PID_Voltage_Loop(_iq Ref,_iq Fdb)
{
   PID_Voltage.Ref=Ref;
   PID_Voltage.Fdb=Fdb;
   PID_Voltage.calc(&PID_Voltage);
}

void PID_Current_Loop(_iq Ref,_iq Fdb)
{
   PID_Current.Ref=Ref;
   PID_Current.Fdb=Fdb;
   PID_Current.calc(&PID_Current);
}