PID.c

/*          PID CONTROL ALGORITHM
** /desc    Algoritimo para Controlador Proporcional Integrativo Diferencial.
**  ref1: https://www.scilab.org/discrete-time-pid-controller-implementation
**  ref2: https://scilabdotninja.wordpress.com/scilab-control-engineering-basics/module-4-pid-control/
** /var r é o valor desejado para a saída
** /var y é o valor da saída
** /ret     retorna a ação de controle
*/
float pidVo(float r, float y){
    	// PID SETUP FROM ZNFD METHOD:
	const float Ku = 0.02;
	const float Tu = 3.0155e-3;

#define CLASSICAL			0
#define PESSEN 			1
#define SOME_OVERSHOOT 	2
#define ZERO_OVERSHOOT 	3
#define PI				4
#define PD				5
#define FIND_KU			6
#define CUSTOM1			7
#define CUSTOM2			8
#define BYPASS			9

#define PID CUSTOM1

	// PID VARIABLES
#if PID  == CLASSICAL
	    const float Kp = 0.6*Ku;
	    const float Ki = 1.2*Ku/Tu;
	    const float Kd = 3*Ku*Tu/40;
#elif PID == PESSEN
	    const float Kp = 7*Ku/10;
	    const float Ki = 1.75*Ku/Tu;
	    const float Kd = 21*Ku*Tu/200;
#elif PID == SOME_OVERSHOOT
	    const float Kp = Ku/3;
	    const float Ki = 0.666*Ku/Tu;
	    const float Kd = Ku*Tu/9;
#elif PID == ZERO_OVERSHOOT
	    const float Kp = Ku/5;
	    const float Ki = (2/5)*Ku/Tu;
	    const float Kd = Ku*Tu/15;
#elif PID == PI
	    const float Kp = 0.45*Ku;
	    const float Ki = 0.54*Ku/Tu;
	    const float Kd =0;
#elif PID == PD
	    const float Kp = 0.1*Ku;
	    const float Ki = 0;
	    const float Kd = Ku*Tu/10;
#elif PID == FIND_KU
	    const float Kp = Ku;
	    const float Ki = 0;
	    const float Kd = 0;
#elif PID == CUSTOM1
	    const float Kp = 0.5*Ku;
	    const float Ki = 1.0*Ku/Tu;
	    const float Kd = 0.15*Ku*Tu;
#elif PID == CUSTOM2
	    const float Kp = 0.5*Ku;
	    const float Ki = 1.0*Ku/Tu;
	    const float Kd = 0.15*Ku*Tu;
#elif PID ==  BYPASS
	    const float Kp = 1;
	    const float Ki = 0;
	    const float Kd = 0;
#else
	    const float Kp = 0;
	    const float Ki = 0;
	    const float Kd = 0;
#endif
#undef PID

	    const float N = 10;
	    const float Ts = PERIOD;

	    // Difference equation coefficients:
	        const float a0 = 1 +N*Ts;
	const float a1 = -(2 + N*Ts);
	const float a2 = 1;
	const float b0 = Kp*(1+N*Ts) +Ki*Ts*(1+N*Ts) +Kd*N;
	    const float b1 = -(Kp*(2+N*Ts) +Ki*Ts +2*Kd*N);
	    const float b2 = Kp +Kd*N;
	    const float ku1 = a1/a0; 
	    const float ku2 = a2/a0; 
	    const float ke0 = b0/a0; 
	    const float ke1 = b1/a0; 
	    const float ke2 = b2/a0;
	    static float e0 = 0;
	    static float e1 = 0;
	    static float e2 = 0;
	    static float u0 = 0;
	    static float u1 = 0;
	    static float u2 = 0;

	    // Update error and control variables
	    e2 = e1;
	    e1 = e0;
	    u2 = u1;
	    u1 = u0;

	    // Compute error:
	    e0 = r -y;

	    // Compute control action:
	    u0 = -ku1*u1 -ku2*u2 +ke0*e0 +ke1*e1 +ke2*e2;

	// Anti windup
	if(u0 < D_MIN)	   	u0 = D_MIN;
	else if(u0 > D_MAX)	u0 = D_MAX;

	    return u0;
}