How to build a simple PID algorithm - Part 3

Read up wiki’s page on PID control before you continue. Now we’ll build the complete PID algorithm.

We start with calculation of the sensor sum and average similar to the above code. Note the next few segments of code are not complete. You’ll have to finish them yourself!

sensors_average = 0;
sensors_sum = 0;  
for (int i = 0; i < 5; i++) {    
  sensors[i] = analogRead(i);      
  sensors_average += sensors[i] * i * 1000;//Calculating the weighted mean of the sensor readings  
  sensors_sum += int(sensors[i]);             //Calculating sum of sensor readings
}  
void pid_calc() {
  position = int(sensors_average / sensors_sum);  
  proportional = position – set_point;      // Replace set_point by your set point  
  integral = integral + proportional;  
  derivative = proportional - last_proportional;  
  last_proportional = proportional;
  error_value = int(proportional * Kp + integral * Ki + derivative * Kd);
}

The above formula for calculation of error value is the functional definition of PID control. Notice you have to define the values of Kp, Ki and Kd in the code somewhere. After calculating the value of error, we need to tell the motor to move such that the error is minimized.

void calc_turn() {
  //Restricting the error value between +256
  if (error_value < -256){  
    error_value = -256;
  }    
  if (error_value > 256){  
    error_value = 256;
  }
  // If error_value is less than zero calculate right turn speed values
  if (error_value < 0){  
    right_speed = max_speed + error_value;  
    left_speed = max_speed;
  }
  // If error_value is greater than zero calculate left turn values     else{
    right_speed = max_speed;
    left_speed = max_speed - error_value;
  }
} 

The above code snippet assumes you’re using the differential drive system, where you execute a left turn if you reduce the speed of your left motor and a right turn if you reduce the speed of the right motor. We use a value max_speed that has to be defined by you right in the beginning to control the speed of the motor. The maximum value of this is 256, which corresponds to the maximum output of the 8 bit DAC converter on the Arduino.

Now we have only one job left to do, to run the motors! So let’s define a function which does exactly that. ‘motor_right’ and ‘motor_left’ are the pin numbers at which your motors are connected via the motor driver. Remember to use PWM pins!

void motor_drive(int right_speed, int left_speed) {      // Drive motors according to the calculated values for a turn   analogWrite(motor_right, right_speed);  analogWrite(motor_left, left_speed);  delay(50);           // Optional}

IMPORTANT – If you’re motors don’t run at the same speed, meaning it veers to a side even with both motors get the same power, add a line of code to correct it. This is a rare case, but it may happen nevertheless!

Say your left motor moves faster than your right motor, add this before the analogWrite().

Left_speed = left_speed – 20 

The ‘20’ is a random number, and you should set it depending on your motors.

Put the functions together, and use this statement in the loop section.

void loop() {
  sensors_read(); // Reads sensor values and computes sensor sum and weighted average  pid_calc(); // Calculates position[set point] and computes Kp,Ki and Kd
  calc_turn(); // Computes the error to be corrected
  motor_drive(right_speed, left_speed); // Sends PWM signals to the motors   }

That’s it! You’ve built your PID algorithm for your line follower! You’ve yet to complete the code with the definitions of all the variables and the setup() segment.


Source: Read GuideToBuildAutoRobots