EmbeddedRelated.com
Code Snippets
   Code Snippets    Submitted by wpkeenan

PID Control

● March 23, 2013●1 comment ● Coded in C 
// PID Control program by Bill Keenan, Melbourne

// Throttle PID loop stuff
#define Umin -1022
#define Umax 1022

#define Kp  0.5	//
#define Ki 0.005 //
#define Kd 1.25 	//
float U0 = 0;
float integ;  	// integration part of PID-loop must be saved 
float ep;		// previous error used for differential error (e - ep) 
 

/*****************************************************************************/
float prop, junk, der;      //  
//	junk, U0, Umax, Umin, Kp, Ki, err. 
int throttle_pidloop(int realpos) // PID-loop calc 
                          // integ - integral part of controller 
                          // ep - preceding control error 
                          // realpos - throttle position feedback 
// Besides this function uses global variables such as:
//                    Kp,Ki,Kd,U0 - constants of controller;
//                    accel_use - setpoint
//                    Umin,Umax - range of the output control signal 
{
int output;
float error;		// actual control error 

error = (float)(accel_use - realpos); // calculation of actual error 

der = ((error - ep) * Kd);
prop = error * Kp;  // calculation of proportional part 
if(prop > 1022)
	{
	prop = 1022;
	}
if(prop < -1022)
	{
	prop = -1022;
	}
junk = error * Ki;	// calculation of integral part 
integ = integ + junk;	 

if ((int)integ > Umax) 
	{
	integ = (float)Umax;
	}
else if ((int)integ < Umin) 
	{
	integ = (float)Umin;
	}

output = (int)(prop + integ + der); 

if (output > Umax) 
	{
	output = Umax;          
	}
 else if (output < Umin) 
 	{
	output = Umin;
	}
ep = error;                          		// new error 
return(output);        // return the object controlling magnitude 
}