Here is some pseudocode for a full PID controller. (I tend to use standard c with the AVR-GCC compiler and have the C files. If somebody is interested I could post them). It is as general as it can be because PID can be used to control many things besides motors.
-Lets say we have a routine named setActuator() that takes care of giving a voltage command to the motor (it could be an analogWrite)
-Also a routine called getSensor() that returns the value from the sensor we have attached to the actuator (potentiometer, encoder etc). This could be an analogRead() in case of a potentiometer ie.
- We will need some variables named: sensorValue, actuatorValue, error, previousError, integralPart.
- We will need some constants named pGain, dGain, iGain, antiWindUpLimit
If all variables and constants are not clear yet don't worry I will explain them one by one
Here is the pseudocode (it looks like C) that implements the PID.
(It runs inside a routine called in the main loop)
sensorValue = getSensor();
previousError = error; //store the previous error
error = sensorValue - actuatorValue; //this is how far we are from our goal
integralPart = integralPart + error; //this is the integral part which eliminates the steady state error
if(integralPart > antiWindUpLimit) then //here we limit the integral part
integralPart = AntiWindUpLimit;
if(integralPart < antiWindUpLimit) then
integralPart = -AntiWindUpLimit;
//this is the PID calculation
actuatorValue = pGain*(error + dGain*(error - previousError) + iGain*integralPart);
//this can also be written like this
actuatorValue = pGain*error + dGain*(error - previousError) + iGain*integralPart;
//just expanding the multiplication to understand how the change of each gain affects our system better.
- pGain*error is the proportional term and it acts like this: the further we are from our goal the bigger the voltage to the motor (or the command to the actuator) - but when we get close to our goal (setPoint) there is a very small voltage - maybe not enough to move the motor and certainly not enough to keep the motor in place if we try to move it by hand.
- This is where the integral term = iGain * integralPart comes in. If we are close to our goal but have not reached it, at every loop cycle the integral part will become bigger and bigger until it moves the motor in place. Also if we try to take the motor out of its position this term also becomes bigger and bigger until the motor moves back in place. So why the antiWindUp (= limit of the integral part)? and what is it?... Imagine this: we are stronger than the motor and we keep it with our hand out of place even if the integralPart does its thing and commands the motor back in place by the full supply voltage available to the motor. The PID loop goes on increasing the integral term to infinity as time passes... This is not good and also it is pointless... The motor after all can be activated only to 100% of the voltage we have available. So usually we limit the integral term to 100% of the values the setActuator() function can handle (or less). If we dont and a situation like the above happens we will surely overshoot our goal when we release the motor.
- Finally the derivative term. Up to now we have the proportional term which behaves like this: Bigger command the further we are away from our goal - not much when we are close. The integral term which gets bigger the longer
we are away from our goal. These two guys don't care how fast we are going towards our goal and if we will overshoot the target - the derivative term does. Look at the calculation - it is exactly this: how fast are we approaching? ( error - prevError ). So the faster we are approaching the derivative term tries to limit the actuatorValue so that we do not overshoot.
The above are a bit simplistic but say what each term does when it is working as it should. Note that THE key factor to PID control is how frequently we execute the calculations. A PID loop at 1KHz is usually enough. A PID loop at 2KHz is better and so on.
I hope this helped
. I have been implementing control systems for some time now and it is how I understand the PID inside my head. Once you understand it it is really simple - and ingenious at the same time