Sunday, December 5, 2010

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.

4 comments:

  1. hi,

    would love to see the source code of your controller. have you hosted it anywhere by any chance ? i cant find any links.

    cheers

    ReplyDelete
  2. hi mate,

    Is there any way you can tell me how "m->velocity_setpoint" works ?
    I know its in encoder ticks but I cant quite picture how this fits into velocity term ?

    Do you have the source code for the whole project ? Im doing something as a university project and would like to have a look at your project files. the link at the top seems to be broken.

    ReplyDelete
  3. Hey, what happened with your blog? Also, there is no link to the source code. Is that on purpose? Do you have code available?

    ReplyDelete