www.eXtremeElectronics.co.in

Main LFR Algorithm

AVR ATmega8 Bases Line Follower.

Easy to make PID LFR Robot.

The main program begins by Initializing three subsystems namely Motor, LED and ADC (for sensor input). The code is as follows.

 //Initialize Motors subsystem.
 MotorInit();
 
 //Initialize LED subsystem
 LEDInit();
 
 //Initialize Analog to Digital Converter (ADC)
 InitADC();

Then program enters into a infinite loop ( while(1) { //Main LFR Loop } ), this infinite loop keeps the robot follow line as long as it has power.

In the loop first thing we do is to read the sensor using the ReadSensors() we get a value between 1 to 5 as follows.

In case a line is NOT found we below any sensor we used value we got last time. This is done by storing the current line position in a variable sprev just before the end of main loop. This sprev is used as previous line position for the next loop.

Code:

 s=ReadSensors();
 //If line is not found beneath any sensor, use last sensor value. 
 if(s==0xFF)
 {
 	s=sprev;
 }

Now a PID algorith is used to find out the control variable from the current position and required position.

Current Position it is the position of line as read by the sensors.

Required Position is 3 (to keep the line on the middle sensor whoes number is 3)

Code:

control = PID(s,3.0);

Then we make the control varriable come within a range of -510 to +510

Code:

 //Limit the control
 if(control > 510)
 control = 510;
 if(control < -510)
 control = -510;

When the control variable is more than 0 that means line is towards the left, so we need to take right turn to correct the error and bring the robot back to track. To do a right turn we need to make the right motor go slow by the amount of control (if control is less than 255).

Code:

MotorA(MOTOR_CCW,255-control);

if control is more than 255 then we need to make the right motor go in opposite direction by the amount of control.

This will create a much faster right turning.

MotorA(MOTOR_CW,control-255);

Similarly if control variable is less than 0 that means line is towards the right, so we need to take left turn to correct the error and bring the robot back to track. The whole code looks like this.

		if(control > 0.0)//the left sensor sees the line so we must turn right
{
if(control>255)
MotorA(MOTOR_CW,control-255);
else
MotorA(MOTOR_CCW,255-control);

MotorB(MOTOR_CW,255);
}
if(control <= 0.0)//the right sensor sees the line so we must turn left
{
if(control<-255)
MotorB(MOTOR_CCW,-(control+255));
else
MotorB(MOTOR_CW,255+control);

MotorA(MOTOR_CCW,255);
}

For more information turning the robot please see differential drive mechanism.

Finally we delay a bit for next cycle and store the current line position (s) to variable sprev for used in next cycle.

 //Delay 
 DelayMs(delay);
 
 sprev=s;

Whole LFR Code Listing


/*
 * LFRM8.c
 *
 * Created: 5/26/2012 7:59:45 PM
 *  Author: Avinash Gupta
 */

#include <avr/io.h>
#include <util/delay.h>

#include "lib/adc/adc.h"
#include "lib/motor/motor.h"
#include "lib/led/led.h"

#define SENSOR_THRES 800

//Map Sensor Number to ADC Channel
#define SENSOR1 0
#define SENSOR2 1
#define SENSOR3 2
#define SENSOR4 3
#define SENSOR5 4

//Gloabal varriables
float pGain = 200;   //Proportional Gain
float iGain =  0.2;  //Integral Gain
float dGain =  120;  //Differential Gain
int delay = 10;

int32_t eInteg = 0;  //Integral accumulator
int32_t ePrev  =0;      //Previous Error


void  DelayMs(uint8_t ms);
float ReadSensors();
float PID(float cur_value,float req_value);


float control;
float s;

int main(void)
{
   //Initialize Motors subsystem.
   MotorInit();

   //Initialize LED subsystem
   LEDInit();

   //Initialize Analog to Digital Converter (ADC)
   InitADC();


    while(1)
    {
      //Previous Sensor Reading
      float sprev;

      //Take current sensor reading
      //return value is between 0 to 5
      //When the line is towards right of center then value tends to 5
      //When the line is towards left of center then value tends to 1
      //When line is in the exact center the the valeue is 3
        s=ReadSensors();

      //If line is not found beneath any sensor, use last sensor value. 
      if(s==0xFF)
      {
         s=sprev;
      }

      //PID Algorithm generates a control variable from the current value
      //and the required value. Since the aim is to keep the line always
      //beneath the center sensor so the required value is 3 (second parameter)
      //The first argument is the current sensor reading.
      //The more the difference between the two greater is the control variable.
      //This control variable is used to produce turning in the robot.
      //When current value is close to required value is close to 0.
      control = PID(s,3.0);

      //Limit the control
      if(control > 510)
         control = 510;
      if(control < -510)
         control = -510;

      if(control > 0.0)//the left sensor sees the line so we must turn right
      {
         if(control>255)
            MotorA(MOTOR_CW,control-255);
         else
            MotorA(MOTOR_CCW,255-control);

         MotorB(MOTOR_CW,255);
      }
      if(control <= 0.0)//the right sensor sees the line so we must turn left
      {
         if(control<-255)
            MotorB(MOTOR_CCW,-(control+255));
         else
            MotorB(MOTOR_CW,255+control);

         MotorA(MOTOR_CCW,255);
      }

      //Delay     
      DelayMs(delay);

      sprev=s;
    }
}

void DelayMs(uint8_t ms)
{
   uint8_t i;
   for(i=0;i<ms;i++)
   {
      _delay_ms(1);
   }
}

//Implements PID control
float PID(float cur_value,float req_value)
{
  float pid;
  float error;

  error = req_value - cur_value;
  pid = (pGain * error)  + (iGain * eInteg) + (dGain * (error - ePrev));

  eInteg += error;                  // integral is simply a summation over time
  ePrev = error;                    // save previous for derivative

  return pid;
}

float ReadSensors()
{
   uint16_t eright,right,middle,left,eleft;
   uint8_t     sensor1,sensor2, sensor3, sensor4,sensor5;

   float avgSensor = 0.0;

   eright=ReadADC(SENSOR5);
   if(eright>SENSOR_THRES)//Right black line sensor
   {
      sensor5 = 1;
      LEDOn(5);
   }
   else
   {
      sensor5 = 0;
      LEDOff(5);
   }

   // Read analog inputs
   right=ReadADC(SENSOR4);
   if(right>SENSOR_THRES)//Right black line sensor
   {
      sensor4 = 1;
      LEDOn(4);
   }
   else
   {
      sensor4 = 0;
      LEDOff(4);
   }

   middle=ReadADC(SENSOR3);
   if(middle>SENSOR_THRES)// Middle black line sensor
   {
      sensor3 = 1;
      LEDOn(3);
   }
   else
   {
      sensor3 = 0;
      LEDOff(3);
   }

   left=ReadADC(SENSOR2);
   if(left>SENSOR_THRES)// Left black line sensor
   {
      sensor2 = 1;
      LEDOn(2);
   }
   else
   {
      sensor2 = 0;
      LEDOff(2);
   }

   eleft=ReadADC(SENSOR1);
   if(eleft>SENSOR_THRES)// Left black line sensor
   {
      sensor1 = 1;
      LEDOn(1);
   }
   else
   {
      sensor1 = 0;
      LEDOff(1);
   }


   if(sensor1==0 && sensor2==0 && sensor3==0 && sensor4==0 && sensor5==0)
   {
      return 0xFF;
   }

   // Calculate weighted mean
   avgSensor = (float) sensor1*1 + sensor2*2 + sensor3*3 + sensor4*4 + sensor5*5 ;
   avgSensor = (float) avgSensor / (sensor1 + sensor2 + sensor3 + sensor4 + sensor5);

   return avgSensor;
}

Building and running the Main LFR Code.

 

  1. Make an Atmel Studio 6 Project Named LFRM8
  2. Add libraries motor,led and adc.
  3. Load the LFRM8.c file and copy/paste the above code.
  4. Build the project.
  5. Burn the hex file to the board.
  6. Assemble the line following robot.

Return to Help Index.