PID Velocity Motor Controller Code Breakdown

In the previous PID Velocity Motor Controller post I discussed the project hardware and project goals. In this post I want to share some breakdown of the PID code as it stands today.

The following code examples are for Atmel AVR microprocessors using the gcc-avr compiler and tool chain.

Encoder Structure:

 typedef struct{    
   int direction;    // Encoder rotation direction.  
   int count;        // Encoder count.  
 } encoder, *pencoder;  
   
 encoder REncoder;  
 encoder LEncoder;  

Motor Structure:

typedef struct{  
   float kp;                // Proportional gain  
   float ki;                // Intigral gain  
   float kd;                // Dirivitive gain  
   float velocity_setpoint; // Motor velocity setpoint in encoder counts per control loop  
   float velocity;          // Motor velocity in encoder counts per control loop  
   float previous_error;    // Motor's previous proportional error  
   float integral_error;    // Motor's integral error  
 } motor, *pmotor;  
   
 motor RMotor;  
 motor LMotor;

Encoder Count Updating:
The encoder count function is called from an interrupt that occurs on a logical change on the quadrature encoders output A. Based off the logic level of the encoders A and B output we can determine the encoders rotation direction and update encoder counts.

void encoder_update(encoder *e, int A, int B){  

   if(A == 1){  
     // Rising edge of A.  
   
     if(B == 1){  
       e->direction = 1;  
       e->count ++;  
     }  
     else{  
       e->direction = 0;  
       e->count --;  
     }  
   
   }  
   else{  
     // Falling edge of A.  
        
      if(B == 1){  
        e->direction = 0;  
        e->count --;  
      }  
      else{  
        e->direction = 1;  
        e->count ++;  
      }  
   }  
 }  

Encoder Velocity Calculation:
To calculate the rotational velocity of the encoder we simply take the absolute value of the encoder counts per program loop.

int encoder_velocity(encoder *e){  
   
   // Velocity in encoder counts per control loop.  
   int velocity = abs(e->count);  
   
   // Reset encoder's count variable.  
   e->count = 0;  
   
   // Return the calculated velocity.  
   return velocity;  
 }  

PID Function:
This function demonstrates just how simple the implementation of the PID control can be. The comments should be self explanatory.

float do_pid(motor *m){  
   
   float p_error;  // The difference between the desired position (position_setpoint) and actual position.  
   float i_error;  // The sum of errors over time.  
   float d_error;  // The difference between the previous proportional error and the current proportional error.  
   
   float p_output;  // The proportional component of the output.  
   float i_output;  // The integral component of the output. This term dampens the output to prevent overshoot and oscillation.  
   float d_output;  // The derivative component of the output. This term is responsible for accelerating the output if the error is large.  
   
   float output;  // The sum of the proportional, integral and derivative terms.  
   
   // Calculate the three errors.  
   p_error = m->velocity_setpoint - m->velocity;  
   i_error = m->integral_error;  
   d_error = p_error - m->previous_error;  
   
   // Calculate the three components of the PID output.  
   p_output = m->kp * p_error;  
   i_output = m->ki * i_error;  
   d_output = m->kd * d_error;  
   
   // Sum the three components of the PID output.  
   output = p_output + i_output + d_output;  
   
   // Update the previous error and the integral error.  
   m->previous_error = p_error;  
   m->integral_error += p_error;  
   
   return output;  
 }  

Adjusting Motor Output:
Here the motor output is adjusted based off the direction of the motor, the input (previous motor output) and adjust (result from the PID function).

int motor_output(int direction, int input, float adjust){  
   
   float output;  // Motor output. Between 0 and 255.  
   
   // Based on direction the either add or suptract the adjust from input.  
   if (direction == 1) { output = input + adjust; }  
   else { output = input - adjust; }  
   
   // Limit the output to between 0 and 255.  
   if (output >= 255) { output = 255; }  
   else if (output <= 0) { output = 0; }  
   
   return (int) output;  
 }  

You can download the complete project files for the PID velocity motor controller here.