Creating a PID controller

From FIRSTwiki

Programming a simple PID controller for a FIRST robot is a relatively easy task. Depending on its use, PID controllers may make the robot much easier to control.

Software

Variable Declaration

Three constants or variables that are the reciprocal of the ratio, derivative, and integral constants of proportionality must be declared and given values to. You may use #define.

PID Function

Arguments to the PID function:

• An integer request value
• The actual value of the device from the current frame
• A pointer to an integer denoting the actual value 1 frame ago
• A signed integer value denoting running total of the error

Preconditions for the PID function:

• All of the above arguments are valid
• Three constants or variables with the reciprocals of the ratio, integral, and derivative constants of proportionality

Postconditions for the PID function:

• The value at the pointer to the last actual value now equals the current actual value
• The sum pointer's value has been incremented by the current error
• The return value is the output value for the device as determined by PID controller logic.

Steps taken in the PID function:

1. Calculate the error (not absolute value of error) as difference between request and actual
2. Add to the requested value the error multiplied by the ratio constant of proportionality
3. Add to the running sum the current error
4. Add the running sum multiplied by the integral constant of proportionality to the requested value
5. Add the derivative constant of proportionality multiplied by the difference between the actual value this frame and the actual value last frame to the requested value
6. Return the requested value (it has been updated due to the previous steps)

An example PID function is (from the now defunct NRG Code Repository (Code Package 1). Check frCoder for updates.):

 #define PID_K_PROP 1
 #define PID_K_INT 1
 #define PID_K_DERIV 1
 
 int PID(int request, int actual, int *lastActual, signed int *sum){
 int pid=request;
 signed int error=request-actual;
 /* Add the "proportional" part of the error */
 pid += error/PID_K_PROP;
 
 /* Add the "integral" part of the error */
 (*sum)+=error;
 pid+=(*sum)/PID_K_INT;
 
 /* Add the "derivative" part of the error */
 pid+=(actual-(*lastActual))/PID_K_DERIV;
 (*lastActual)=actual;
 
 return pid;
 }