pid controller with out derivative filter pid with derivative filter forward Euler, backward Euler , trapezoidal s to z plane mapping

1. /*********************************************************************
* Source code : file name pid.c
* By : 94730 7403553 /Yared Tadesse/
* 11:35PM Tuesday,July 26,2016 GC
* 5:35 Mishit,Maksegno , Hamle 17,2008 EC
* Addis Ababa ,Ethiopia
* reference:
* PID controller­Wikipedia, the free encyclopedia
https://en.wikipedia.org/wiki/PID_controller ..07/26/2016 02:06PM
* Karl Johan Astrom and Richard M. Murray ,"Feedback systems : an introduction
for scientists and engineers ",2009,Princeton University Press,Version
v2.10b,pp306­pp308
* I.Kar,"Digital Control;Module 1: Introduction to Digital Control Lecture Note
2",PP1­4
* Xin­lan Li,Jong­Gyu Park,& Hwi­Beom Shin,"Comparison and Evaluation of Anti­
Windup PI Controllers",Journal of Power Electronics, Vol. 11, No. 1, January
2011
********************************************************************/
#include <math.h>
#include "pid.h"
#define abs(i) (i)<0 ? ­(i) : (i)
/////////////////////////////////////////////////////////////////////
////////////////////////Global_Variables/////////////////////////////
/////////////////////////////////////////////////////////////////////
parameters pidv;
parameters *pid=&pidv;
/////////////////////////////////////////////////////////////////////
///////////////////////End_Of_Global_Variable_Declaration////////////
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
////////////////PID Controller implementation Codes//////////////////
/////////////////////////////////////////////////////////////////////
void setReference(double reference)
{
(*pid).setPoint=reference;
}//end of set point
void setInputBorder(double minima,double maxima)
{
(*pid).minInput=minima;
(*pid).maxInput=maxima;
}//end of setInputBorder
void setOutputBorder(double minima,double maxima)
{
(*pid).minOutput=minima;
1/9

2. (*pid).maxOutput=maxima;
}//end of setOutPutBorder
//Actuator Model with its Limitation
double actuatorLimit(double *value2check,char what2check)
{
switch(what2check)
{
double err;
case(INPUT):
if((*value2check) > (*pid).maxInput){
err=((*pid).maxInput ­ (*value2check));
(*pid).setPoint=(*pid).maxInput;
return err;
}
else if ((*value2check) < (*pid).minInput){
err=((*pid).minInput ­ (*value2check));
(*pid).setPoint=(*pid).minInput;
return err ;
}
else
return 0;
break;
case(OUTPUT):
if((*value2check) > (*pid).maxOutput){
if((*value2check) > 1000000){(*value2check)=1000000;}
err= ((*pid).maxOutput ­ (*value2check));
(*pid).cka=(*pid).maxOutput;
return err;
}
else if ((*value2check) < (*pid).minOutput){
if((*value2check) < ­1000000){ (*value2check)=­1000000; }
err=((*pid).minOutput ­ (*value2check));
(*pid).cka=(*pid).minOutput;
return err;
}
else
(*pid).cka=(*pid).ck;
return 0;
break;
default :
return 0;
}
return 0;
}//end of verifyValue
2/9

3. double pidCompensator(double kp,double ki,double kd,double dt )
{
(*pid).Kp=kp;
(*pid).Ki=ki;
(*pid).Kd=kd;
(*pid).dt=dt;
//Ki=Kp/Ti and Kd=Kp*Td
//Ti=Kp/Ki and Td=Kd/Kp
//Ti*Td=Kp/Ki*Kd/Kp=Kd/Ki
if((*pid).Kd)
(*pid).Kt=1/sqrt(abs((*pid).Kd/(*pid).Ki));
else
(*pid).Kt=1/sqrt(abs((*pid).Kp/(*pid).Ki));
double dterm,pterm,iterm;
////////////////////Cheking for inputboundery/////////////////////////////////
actuatorLimit(&(*pid).setPoint,INPUT);
//////////////////Calculating_Current_Error///////////////////////////////////
(*pid).ek=(*pid).setPoint ­ getProcessVariable();
/////////////////end_of error_caculation//////////////////////////////////////
pterm=(*pid).Kp*((*pid).ek­(*pid).ekminus1) ;
iterm =((*pid).Ki)*((*pid).dt)*((*pid).ekminus1) +
((*pid).Kt*(*pid).dt*actuatorLimit(&(*pid).ckminus1,OUTPUT));
dterm=(*pid).Kd*( (*pid).ek +(*pid).ekminus2 ­(2*((*pid).ekminus1)) )/
(*pid).dt;
(*pid).ck=(*pid).ckminus1 + pterm + dterm + iterm;
actuatorLimit(&(*pid).ck,OUTPUT);
////////Test Values//////////
////////////////////////////Update_Controller_Variables////////////////////////
(*pid).ckminus1=(*pid).ck;
(*pid).ekminus2=(*pid).ekminus1;
(*pid).ekminus1=(*pid).ek;
///////////////////////////End of controller parameter update//////////////////
return (*pid).cka;
} //end of pidCompensator
void setProcessVariable(double value)
{
(*pid).processVariable=value;
}//end of setProcessVariable
3/9

4. double getProcessVariable(void)
{
return (*pid).processVariable;
}//end of getProcessVariable
void pidIntialization(double ckminus1,double ckminus2,double ekminus1,double
ekminus2)
{
(*pid).ckminus1=ckminus1;
(*pid).ckminus2=ckminus2;
(*pid).ekminus1=ekminus1;
(*pid).ekminus2=ekminus2;
}
double pidGetError(void)
{
return (*pid).ek;
}//end of pidGetError
4/9

5. double pidWithDFilter(double Kp,double Ki,double Kd,double N,double dt,char
Method)
{
(*pid).Kp=Kp;
(*pid).Ki=Ki;
(*pid).Kd=Kd;
(*pid).dt=dt;
(*pid).N=N;
if((*pid).Kd){
(*pid).td=(*pid).Kd/(*pid).Kp;
(*pid).Kt=1/sqrt(abs((*pid).Kd/(*pid).Ki));
}
else{
(*pid).Kt=1/sqrt(abs((*pid).Kp/(*pid).Ki));
}
//////////////////Calculating_Current_Error//////////////////////////////
actuatorLimit(&(*pid).setPoint,INPUT);
(*pid).ek=(*pid).setPoint ­ getProcessVariable();
/////////////////end_of error_calculation/////////////////////////////////
switch(Method)
{
case(FORWARD_EULER):
ForwardEulerPID();
break;
case(BACKWARD_EULER):
BackwardEulerPID();
break;
case(TRAPIZOIDAL):
TrapizoidalPID();
break;
};
////////////////////////////Update_Controller_Variables////////////////////
(*pid).ckminus2=(*pid).ckminus1;
(*pid).ckminus1=(*pid).ck;
(*pid).ekminus2=(*pid).ekminus1;
(*pid).ekminus1=(*pid).ek;
////////////////////End of controller parameter update//////////////////
return (*pid).cka;
}//end of pidWithFilter
5/9

6. void ForwardEulerPID(void)
{
double dterm,pterm,iterm,sterm;
double NTsminus1=(*pid).N*(*pid).dt­1;
double NTsminus2=(*pid).N*(*pid).dt­2;
pterm=(*pid).Kp*((*pid).ek + ((NTsminus2*(*pid).ekminus1)­
(NTsminus1*((*pid).ekminus2))));
iterm =(*pid).Ki*((*pid).dt)*((*pid).ekminus1­(NTsminus1*(*pid).ekminus2));
sterm =(*pid).Kt*((*pid).dt)*(actuatorLimit(&(*pid).ckminus1,OUTPUT)­
(NTsminus1*actuatorLimit(&(*pid).ckminus2,OUTPUT)));
dterm=(*pid).Kd*((*pid).N)*((*pid).ek + (*pid).ekminus2 ­ (2*(*pid).ekminus1));
(*pid).ck=pterm + iterm + sterm + dterm + (NTsminus1*(*pid).ckminus2)­
(NTsminus2*(*pid).ckminus1);
////////////////////////////Update_Controller_Variables////////////////////
(*pid).ckminus2=(*pid).ckminus1;
(*pid).ckminus1=(*pid).ck;
(*pid).ekminus2=(*pid).ekminus1;
(*pid).ekminus1=(*pid).ek;
actuatorLimit(&(*pid).ck,OUTPUT);
} //End of pidForwardEuler
void BackwardEulerPID(void)
{
double dterm,pterm,iterm,sterm;
double NTsplus1=(*pid).N*(*pid).dt+1;
double NTsplus2=(*pid).N*(*pid).dt+2;
pterm = (*pid).Kp * ((NTsplus1*(*pid).ek) + (*pid).ekminus2 ­
(NTsplus2*(*pid).ekminus1));
iterm = (*pid).Ki*(*pid).dt * ((NTsplus1*(*pid).ek) + (*pid).ekminus1);
sterm = (*pid).Kt*(*pid).dt * ((NTsplus1*actuatorLimit(&(*pid).ck,OUTPUT))
+ actuatorLimit(&(*pid).ckminus1,OUTPUT));
dterm = (*pid).Kd * (*pid).N * ((*pid).ek ­ (2*(*pid).ekminus1) +
(*pid).ekminus2);
6/9

7.
(*pid).ck = (pterm + iterm + sterm + dterm + (NTsplus2 * (*pid).ckminus1) ­
(*pid).ckminus2 )/NTsplus1;
//updating variable valuse of the controller
(*pid).ckminus2=(*pid).ckminus1;
(*pid).ckminus1=(*pid).ck;
(*pid).ekminus2=(*pid).ekminus1;
(*pid).ekminus1=(*pid).ek;
actuatorLimit(&(*pid).ck,OUTPUT);
} //End of pidBackwardEuler
void TrapizoidalPID(void)/*Usatble and require further study*/
{
double dterm,pterm,iterm,sterm;
double NTsm2p2=(((*pid).N*(*pid).dt)­2)/2;
double NTsp2p2=(((*pid).N*(*pid).dt)+2)/2;
double NT=(*pid).N * (*pid).dt;
pterm = ((NTsp2p2*(*pid).ek)+(2*(*pid).ekminus1)­
(NTsm2p2*(*pid).ekminus2))*(*pid).Kp;
iterm = ((NTsp2p2*(*pid).ek)+(NT*(*pid).ekminus1)+
(NTsm2p2*(*pid).ekminus2))*(*pid).Ki*(*pid).dt/2;
sterm=((NTsp2p2*actuatorLimit(&(*pid).ck,OUTPUT))+
(NT*actuatorLimit(&(*pid).ckminus1,OUTPUT))+
(NTsm2p2*actuatorLimit(&(*pid).ckminus2,OUTPUT)))*(*pid).Kt*(*pid).dt/2;
dterm = ((*pid).ek ­ (2*(*pid).ekminus1) +
(*pid).ekminus2)*(*pid).Kd*(*pid).N;
(*pid).ck=(pterm + iterm + sterm +dterm)/NTsp2p2;
//updating variable valuse of the controller
(*pid).ckminus2=(*pid).ckminus1;
(*pid).ckminus1=(*pid).ck;
(*pid).ekminus2=(*pid).ekminus1;
(*pid).ekminus1=(*pid).ek;
actuatorLimit(&(*pid).ck,OUTPUT);
}//End of pidTrapizoidal
7/9

8. /*
header file : file name pid.h
*///Protection against multiple inclusion
#ifndef _PID_H_
#define _PID_H_
#define INPUT 1
#define OUTPUT 2
#define ERRSUM 3
#define FORWARD_EULER 4
#define BACKWARD_EULER 5
#define TRAPIZOIDAL 6
typedef struct
{
double Kp;
double Ki;
double Kd;
double Kt;
double N;
double bias;
double dt;
double td;//derivative time constant
double setPoint;
double b;
double c;
double maxInput;
double minInput;
double maxOutput;
double minOutput;
double ck;//the control signal at instant k
double cka; //Actuator out put
double ckminus1; //The previous control signal for kminus1 set to 0 at start
double ckminus2;
double ek; //the error signal at instant k
double ekminus1; //the error signal at instant kminus1 set to 0 intially
double ekminus2; //the error signal at instant kminus2 set to 0 intially
double processVariable; //feedBack variable value
} parameters;
//Intialize the parameters to avoid unused or an intialized paamters junk
//////////////////////////////////////////////////////////////////////////
double sqrtOf(double num,double precision);
double absOf(double numb);
//////////////////////////////////////////////////////////////////////////
8/9

9. void setReference(double reference);
void setInputBorder(double minima,double maxima);
void setOutputBorder(double minima,double maxima);
void setProcessVariable(double value);
double getProcessVariable(void);
double actuatorLimit(double *value2check,char what2check);
double pidCompensator(double kp,double ki,double kd,double dt );
void pidIntialization(double ckminus1,double ckminus2,double ekminus1,double
ekminus2);
double pidGetError(void);
double pidWithDFilter(double Kp,double Ki,double Kd,double N,double dt,char
Method);
void ForwardEulerPID(void);
void BackwardEulerPID(void);
void TrapizoidalPID(void);
/////////////////////////GLOBAL VARIABLERS/////////////////////////////////
#endif //end of multiple inclusion protection
9/9