Code Snippets Submitted by wpkeenan
PID Control
// 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
}PID Control
// 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
}
