108 lines
2.8 KiB
C
108 lines
2.8 KiB
C
|
/*=====================================================================================
|
||
|
|
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 "pid_reg3.h"
|
||
|
|
|
||
|
|
#define IQ_100 1677721600
|
||
|
|
|
||
|
|
#pragma CODE_SECTION(pid_reg3_calc,".fast_run");
|
||
|
|
void pid_reg3_calc(PIDREG3 *v)
|
||
|
|
{
|
||
|
|
_iq Ud2;
|
||
|
|
// Compute the error
|
||
|
|
v->Err = v->Ref - v->Fdb;
|
||
|
|
|
||
|
|
// Compute the proportional output
|
||
|
|
v->Up = _IQmpy(v->Kp,v->Err);
|
||
|
|
|
||
|
|
// Compute the integral output
|
||
|
|
v->Ui = v->Ui + _IQmpy(v->Ki,v->Up) + _IQmpy(v->Kc,v->SatErr);
|
||
|
|
/*
|
||
|
|
// Saturate the integral output
|
||
|
|
if (v->Ui > v->OutMax)
|
||
|
|
v->Ui = v->OutMax;
|
||
|
|
else if (v->Ui < v->OutMin)
|
||
|
|
v->Ui = v->OutMin;
|
||
|
|
*/
|
||
|
|
// Compute the derivative output
|
||
|
|
Ud2 = v->Up - v->Up1;// _IQmpy(IQ_100,(v->Up - v->Up1));
|
||
|
|
v->Ud = _IQmpy(v->Kd,Ud2);
|
||
|
|
|
||
|
|
|
||
|
|
// 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;
|
||
|
|
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
#pragma CODE_SECTION(pid_reg3_calc,".fast_run");
|
||
|
|
void pid_reg3_undependent_calc(PIDREG3 *v)
|
||
|
|
{
|
||
|
|
_iq Ud2;
|
||
|
|
// Compute the error
|
||
|
|
v->Err = v->Ref - v->Fdb;
|
||
|
|
|
||
|
|
// Compute the proportional output
|
||
|
|
v->Up = _IQmpy(v->Kp,v->Err);
|
||
|
|
|
||
|
|
// Compute the integral output
|
||
|
|
v->Ui = v->Ui + _IQmpy(v->Ki,v->Err) + _IQmpy(v->Kc,v->SatErr);
|
||
|
|
/*
|
||
|
|
// Saturate the integral output
|
||
|
|
if (v->Ui > v->OutMax)
|
||
|
|
v->Ui = v->OutMax;
|
||
|
|
else if (v->Ui < v->OutMin)
|
||
|
|
v->Ui = v->OutMin;
|
||
|
|
*/
|
||
|
|
// Compute the derivative output
|
||
|
|
Ud2 = v->Up - v->Up1;// _IQmpy(IQ_100,(v->Up - v->Up1));
|
||
|
|
v->Ud = _IQmpy(v->Kd,Ud2);
|
||
|
|
|
||
|
|
|
||
|
|
// 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;
|
||
|
|
|
||
|
|
}
|