Line Following Sensor Board

Line Following Sensor Board

Difficulty: Intermediate

Time: 1 Hour and 30 Minutes

Code Version: 0.6

We will make a line following sensor board using 5 TCRT sensors to detect the line. These sensors detect the level of infrared light reflected from a surface. They consist of 2 parts; an infrared LED and a infrared photo-transistor. The below diagram shows what happens when the sensors are over black and white surfaces;

TCRT 5000 Sensor

We can hook these up to the Arduino analogue pins to detect the amount of infrared light detected. We can then use this information to determine if the sensors are over the white background or a black line. Once this is done MIPR can decide what to do; either move forwards, backwards, left, right or to stop. This should result in MIPR following the line.

Required Components

  • 7cm * 5cm perfboard
  • 5 * 4.7K ohm resistors or 5 * 470 ohm resistors for the high power version
  • 220 ohm resistor or 1 * 20 ohm resistor for the high power version
  • 14 pin female header 2.54mm pitch
  • 5 pin male header for battery board
  • Enamelled wire or hookup wire
  • Castor wheel
  • Solder
  • 1cm stand offs and crews for the castor wheel

 

Required Components
Required Components

Tools Required

  • Soldering Iron.
  • Snips.
  • Solder.
  • Solder sucker.
  • Vice/jig to hold the circuit board.
  • Drill.
  • 3mm Drill bit.

 

Circuit Diagram

SB002
Line following sensor board SB002

High Power vs Low Power

The version of the sensor board shown in the video and schematic is the low power version. This version works really well and each TCRT emitter consumes 4mA of power, however due to this low current draw little IR light is emitted making interference a problem in high noise areas such as outdoors. If your going to use the board in high noise areas use the high power configuration where we supply 50mA to each TCRT emitter. The draw back to this is that it consumes a lot more power so your battery won't last as long and you will need to change your linear regulator in MIPR for a BEC. If you don't do this the linear regulator will overheat and cut out. If you install a BEC ensure that you set it's output to 5V.

To configure the board in high power mode swap the 220 ohm resistor for a 20 ohm resistor and the 4700 ohm resistors for 470 ohm resistors.

Connect the components in accordance with the above diagram. A YouTube video can be seen below showing you how to make the board or you could buy a ready made board from out E-bay shop.

The Completed Sensor Board

Line Following Sensor Board
Line Following Sensor Board

I designed a PCB version of this board to make prototyping easier. I will order some and add them to my E-Bay shop if there's enough interest.

Line Follower PCB
Line Follower PCB

Line Following Code

To put MIPR into line following mode open serial monitor and connect to MIPR. Type a capitol O for Oscar and when asked to select a mode select either 6, 7 or 8. The modes are as follows;

  • 6 = Simple LF algorithm
  • 7 = More complex LF algorithm
  • 8 = PID LF algorithm (NOT IMPLEMENTED YET)

Once the algorithm is selected MIPR will restart, you will need to put MIPR over a clear part of track for the sensors to calibrate, once calibration is complete MIPR will beep twice. This will take around 5 seconds.

In order for the below code to work download all of the code from our GitHub Repo, unzip it then go into the Version_0_6 folder open any of the .ino files  in Arduino IDE then upload the code to your robot. The code uses multiple tabs with code from previous tutorials that have been omitted from this page.

/*
 * Line Follower Module Verison 0.1
 * https://www.l33t.uk/arduino_projects/mipr/
 * Copyright David Bradshaw 2019
 * 
 * This is designed for tracks that use black lines on white background
 * 
 * The below code uses absolute values from the sensor board and for the code
 * to work you will need to use the resistor values from the tutorial for either
 * the Low Power or High Power board. If you use different valued resistors
 * you will need to convert the values; for the below code to work the baseLine
 * values will be between 250 and 450, if your baseLine values are 800 for instance 
 * you will have to divide all sensor values by at least half for them to work with this code
 * 
 *      Resistor values should be; Low Power: R1 220 Ohms, R2 - R6 4700 Ohms
 *                                 High Power: R1 20 Ohms, R2 - R6 470 Ohms (7805CV must be replaced with a BEC due to current draw)
 */

#include <PID_v1.h>
 
int L1 = A5; //Outer Left
int L2 = A4; //Inner Left
int M = A2; //Middle
int R2 = A1; //Inner Middle
int R1 = A0; //Outer Middle

float L1_bias = 1;
float L2_bias = 1;
float R1_bias = 1;
float R2_bias = 1;

int leftOVal;
int rightOVal;
int leftMVal;
int rightMVal;
int midVal;
    
int baseLineValue = 0;

boolean isCald = false;
boolean firstRun = true;

int lrErr = 0;

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

double baseSpeed = 40; //base speed

//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=1, consKd=0.02;

PID leftPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
PID rightPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);

int refreshRate = 0;
long refreshStartTime = millis();

void line_FollowerSetup()
{
    pinMode(A0, INPUT);
    pinMode(A1, INPUT);
    pinMode(A2, INPUT);
    pinMode(A4, INPUT);
    pinMode(A5, INPUT);
    pinMode(A6, INPUT);
    pinMode(A7, INPUT);

    Setpoint = 0;
    //turn the PID on
    leftPID.SetOutputLimits(0, 200);
    leftPID.SetSampleTime(10);
    leftPID.SetMode(AUTOMATIC);
    rightPID.SetMode(AUTOMATIC); 
}

//Simple line follower using just 2 sensors L2 and R2. You need a fairly thick line for this to work properly.
//The robot will use absolute values for the speed of each motor not reacting to the environment just using simnple
//if then else statements to navigate the track
void simple_LF(int leftVal, int rightVal)
{ 
    int leftMax = leftVal + (leftVal * 0.2); //Tolerance of 20% to make the robot less jerky
    int leftMin = leftVal - (leftVal * 0.2);

    if (rightVal > leftMin && rightVal < leftMax) //If the left and right values are within 20%, go forward
    {
        Forwards(100, 100);
    }
    else if (leftVal > rightVal)
    {
        Right(50,50);
    }
    else if (rightVal > leftVal)
    {
        Left(50,50);
    }
}

//A simple line follower using all sensors by averaging out the values for left and right sensors
//The speed is adjusted depending on the difference between left and right values
void simple_LF_M(int lftAv, int rgtAv, int lrErr)
{  
    baseSpeed = 96; //base speed
    int minSpeed = 0; //Minimum motor speed
    int maxSpeed = 255; //Maximum motor speed when using low power sensor board
    //int maxSpeed = 150; //Maximum motor speed when using high power sensor board
    
    int lftSpeed;
    int rgtSpeed;
    
    //Our goal is to get lrErr to 0
    //if it is negative then the robot needs to turn left (left side over the line too much)
    //if it is possitive then the robot needs to turn right (right side over the line too much)

    //lrErr = lrErr * 0.8;      //Only use for high power sensor board
    
    if(lrErr > 0) // turn right if the error is possitive
    {
        lftSpeed = baseSpeed - lrErr;
        rgtSpeed = baseSpeed + lrErr;
    }
    else // turn left if the error is negative or 0
    {
        lftSpeed = baseSpeed + abs(lrErr);
        rgtSpeed = baseSpeed - abs(lrErr);
    }

    //Make sure we are above the minimum speed and below the maximum speed
    if (lftSpeed < minSpeed){lftSpeed = minSpeed;}
    if (rgtSpeed < minSpeed){rgtSpeed = minSpeed;}
    if (lftSpeed > maxSpeed){lftSpeed = maxSpeed;}
    if (rgtSpeed > maxSpeed){rgtSpeed = maxSpeed;}

    float nFactor = 1.8; //If this value is set too low the robot will be very jerky
    //if the speed is nFactor% higher than the base speed then turn left or right, this will produce a sharper 
    //turn than the Forwards function as it will turn one motor forwards and the other backwards to produce the 
    //sharp turn.
    if(lftSpeed > float(float(baseSpeed) * nFactor))
    {
        Left(lftSpeed, rgtSpeed);
    }
     else if(rgtSpeed > float(float(baseSpeed) * nFactor))
    {
        Right(lftSpeed, rgtSpeed);
    }
        
    else
    {
        Forwards(lftSpeed, rgtSpeed);
    }
}


//Complex line follower using a PID and all 5 sensors 
void complex_LF_1(int leftOVal, int leftMVal, int midVal, int rightMVal, int rightOVal, int lrErr)
{
    //  P - Proportional an amount to multiply the error by 
    //  I - Integral the sum of previous errors
    //  D - Derivative the speed at which we react to errors

        /*
        P:instanteneousError = setpoint – input;
        I:cumulativeError = += error * elapsedTime;
        D:rateOfError = (error – errorLastCalculation)/elapsedTime;
         */

    /*
     * For this PID the setpoint will always be 0 i.e. no error between the left and right side
     */
    Input = lrErr; 
    double gap = Setpoint-Input; 

    int lftSpeed;
    int rgtSpeed;
    int errThreshold = 10;
    int upperThreshold = 80;
    
    if (abs(lrErr) > errThreshold) //if we have a small lrErr
    {
         leftPID.Compute(); //Only add to the PID if the error is greater than the threshold
    }

    if (abs(lrErr) > upperThreshold)
    {
        if (baseSpeed > 40)
        {
            baseSpeed = baseSpeed - 0.1;
        }
    }
    if (abs(lrErr) < errThreshold) //if we have a small lrErr
    {
        if (baseSpeed < 200)
        {
            baseSpeed = baseSpeed + 0.01;
        }
        lftSpeed = baseSpeed + Output;
        rgtSpeed = baseSpeed + Output;
    }
    else if(lrErr > 0) // turn right if the error is possitive
    {
        lftSpeed = baseSpeed - Output;
        rgtSpeed = baseSpeed + Output;
    }
    else // turn left if the error is negative or 0
    {
        lftSpeed = baseSpeed + Output;
        rgtSpeed = baseSpeed - Output;
    }

    int minSpeed = 0; //Minimum motor speed
    int maxSpeed = 255; //Maximum motor speed when using low power sensor board
    
    if (lftSpeed < minSpeed){lftSpeed = minSpeed;}
    if (rgtSpeed < minSpeed){rgtSpeed = minSpeed;}
    if (lftSpeed > maxSpeed){lftSpeed = maxSpeed;}
    if (rgtSpeed > maxSpeed){rgtSpeed = maxSpeed;}
    
    Forwards(abs(lftSpeed), abs(rgtSpeed));
    
    Serial.print(gap);
    Serial.print(',');
    Serial.print(lrErr);
    Serial.print(',');
    Serial.print(Output);
    Serial.print(',');
    Serial.print(lftSpeed);
    Serial.print(',');
    Serial.println(rgtSpeed);
}

//Complex line follower using a PID and all 5 sensors 
void complex_LF(int leftOVal, int leftMVal, int midVal, int rightMVal, int rightOVal, int lrErr)
{
    //  P - Proportional an amount to multiply the error by 
    //  I - Integral the sum of previous errors
    //  D - Derivative the speed at which we react to errors

        /*
        P:instanteneousError = setpoint – input;
        I:cumulativeError = += error * elapsedTime;
        D:rateOfError = (error – errorLastCalculation)/elapsedTime;
         */

    /*
     * For this PID the setpoint will always be 0 i.e. no error between the left and right side
     */

     //left vs center error
     int lcErr = ((leftOVal + leftMVal) / 2) - midVal; //A possitive value indicates that the robot needs to move to the right
                                                        //A negative value means that the centre sensor is closer to the line
                                                        
     int rcErr = ((rightOVal + rightMVal) / 2) - midVal; //A positive value indicates that the robot needs to move right
                                                            //A negative value means that the centre sensor is closer to the line
     

    
   
    Input = lrErr;
    //double gap = abs(Setpoint-Input); 
    double gap = Setpoint-Input; 
    leftPID.Compute();

    int lftSpeed;
    int rgtSpeed;
    
    if(lrErr > 0) // turn right if the error is possitive
    {
        lftSpeed = 0 - Output;
        rgtSpeed = 0 + Output;
    }
    else // turn left if the error is negative or 0
    {
        lftSpeed = 0 + Output;
        rgtSpeed = 0 - Output;
    }

    int minSpeed = 0; //Minimum motor speed
    int maxSpeed = 255; //Maximum motor speed when using low power sensor board
    
    if (lftSpeed < minSpeed){lftSpeed = minSpeed;}
    if (rgtSpeed < minSpeed){rgtSpeed = minSpeed;}
    if (lftSpeed > maxSpeed){lftSpeed = maxSpeed;}
    if (rgtSpeed > maxSpeed){rgtSpeed = maxSpeed;}
    
    Forwards(abs(lftSpeed), abs(rgtSpeed));
    Serial.print(gap);
    Serial.print(',');
    Serial.print(lrErr);
    Serial.print(',');
    Serial.print(Output);
    Serial.print(',');
    Serial.print(lftSpeed);
    Serial.print(',');
    Serial.println(rgtSpeed);
}

void LFHandler(int mode, boolean fullTele)
{
     //Check to see if this is the first run, if so calibrate the sensor board
     if (firstRun == true)
     {
         sensor_Cal();
         firstRun = false;
     }

    //get the sensor values
    int RefreshTimer = millis() - refreshStartTime;
    if (RefreshTimer > refreshRate)
    {
        leftOVal = getCalSensorVal(L1);
        rightOVal = getCalSensorVal(R1);
        leftMVal = getCalSensorVal(L2);
        rightMVal = getCalSensorVal(R2);
        midVal =  getCalSensorVal(M);
        RefreshTimer = 0;
        refreshStartTime = millis();
    }


    //average the value for the outer sensors
    int lftAv = (leftOVal + leftMVal) / 2;
    int rgtAv = (rightOVal + rightMVal) / 2;

    int outtertAv = (leftOVal + rightOVal) / 2;
    int innerAv =   (leftMVal + rightMVal) / 2;

    lrErr = lftAv - rgtAv; //Error for the intter left and inner right sensors
    
    int absAv = (leftOVal + rightOVal + leftMVal + rightMVal + midVal) / 5;

    float lftSpeed = 0;
    float rgtSpeed = 0;

    //---------------------------------------------------------------------------------------------------------------------------------------------------------------
    //THE BELOW CODE IS ALL ABOUT DETECTING A LINE. IF THE LINE IS THERE THEN THE ALGORITHM WILL BE EXECUTED OTHER WISE THE ROBOT WILL STOP AND TURN ON THE SPEAKER
    //---------------------------------------------------------------------------------------------------------------------------------------------------------------
    
    boolean detectLine = false; //can we see a line
    
    //Compare readings from all sensors to see if a line is present we should have a decrease of at least 10% if we can see a line
    float lineThresh = 0.90; //This value might need tuning depending on the surface
    
    if (lftAv < 50 && rgtAv < 50) //No IR detected so we are either in a black surface, upside down or theres a thick cross road
    {
        //no floor can't be detected
        detectLine = false;
        speaker_on();
    }
    else if(lftAv >  rgtAv)  //Line could be on the right if the robot
    {
        if (float(float(rgtAv) / float(lftAv)) > lineThresh )
        {
            //No line detected check middle sensor
            if (checkMid(midVal, lftAv, rgtAv) == true) //No line detected in the right side so check the middle sensor
            {
                //line detected
                detectLine = true;
            }
            else
            {
                detectLine = false;
            }
        }
        else //RIGHT SENSORS OVER THE LINE
        {
            //line detected
            detectLine = true;
        }
    }
    else if(lftAv <  rgtAv) //Line coule be on the left of the robot
    {
        if (float(float(lftAv) / float(rgtAv)) > lineThresh )
        {
            //No line detected check middle sensor
            if (checkMid(midVal, lftAv, rgtAv) == true) //No line detected in the left side so check the middle sensor
            {
                //line detected
                detectLine = true;
            }
            else
            {
                detectLine = false;
            }
        }
        else  //LEFT SENSORS OVER THE LINE
        {
            //line detected
            detectLine = true;
        }
    }

    //---------------------------------------------------------------------------------------------------------------------------------------------------------------
    //------------------------------------------- END OF LINE DETECTION STUFF ---------------------------------------------------------------------------------------
    //---------------------------------------------------------------------------------------------------------------------------------------------------------------

    if (detectLine == false) //No line detected reverse the robot and sound the speaker
    {
        //softStop();
        Backwards(42,42);
        speaker_on();
    }
    else //We have a line so execute the correct algorithm
    {
        if(mode == 1) //Simple LF
        {
            speaker_off();
            simple_LF(leftMVal, rightMVal);
        }
        else if (mode == 2) //Intemediate LF
        {
            speaker_off();
            simple_LF_M(lftAv, rgtAv, lrErr);
        }
        else if (mode == 3) //PID
        {
            speaker_off();
             complex_LF_1(leftOVal, leftMVal, midVal, rightMVal, rightOVal, lrErr);

        }
    }
}

boolean checkMid(int midVal, int lftAv, int rgtAv)
{
    float lineThresh = 0.95; //This value might need tuning
    //Average left and right values here for simplicity
    if (float(float(midVal) / float(((lftAv + rgtAv) / 2))) > lineThresh)
    {
        //No line detected
        return false;
    }
    else  //MID SENSOR OVER THE LINE
    {
        //line detected
        return true;
    }
}

//Returns raw value from a sensor
int getRawSensorVal(int sensor)
{
    return analogRead(sensor);
}

//Returns calibrated value from a sensor
int getCalSensorVal(int sensor)
{
    float sensorVal;

    if(sensor == M){sensorVal = (float)analogRead(sensor);}
    else if(sensor == L1){sensorVal = L1_bias * (float)analogRead(sensor);}
    else if(sensor == L2){sensorVal = L2_bias * (float)analogRead(sensor);}
    else if(sensor == R1){sensorVal = R1_bias * (float)analogRead(sensor);}
    else if(sensor == R2){sensorVal = R2_bias * (float)analogRead(sensor);}

    return (int)sensorVal;
}

//Calibration routine to calibrate the sensors accounting for impedance mismatches in the circuit
//and for IR reflectance properties of different materials.
void sensor_Cal()
{
    //One beep indicates that Cal is about to start
    speaker_on();
    delay(450);
    speaker_off();
    
    Serial.println("Put all sensors over a white background and wait for a beep");
    delay(500);

    int bechmarkM = 0; //Benchmark value all other sensors will align themselves with this value

    int itterations = 250;
    long averageVal = 0;

    //get middle average value
    for (int ii = 0; ii < itterations; ii++)
    {
        delay(1);
        averageVal = averageVal + (long)getRawSensorVal(M);
    }
    bechmarkM = averageVal / itterations;

     long averageValL1 = 0; long averageValL2 = 0; long averageValR1 = 0; long averageValR2 = 0;
     
    //Bias values that will be used to align the senor values
    for (int ii = 0; ii < itterations; ii++)
    {
        delay(1);
        averageValL1 = averageValL1 + (long)getRawSensorVal(L1);
        averageValL2 = averageValL2 + (long)getRawSensorVal(L2);
        averageValR1 = averageValR1 + (long)getRawSensorVal(R1);
        averageValR2 = averageValR2 + (long)getRawSensorVal(R2);
    }

    averageValL1 = averageValL1 / itterations;
    averageValL2 = averageValL2 / itterations; 
    averageValR1 = averageValR1 / itterations;
    averageValR2 = averageValR2 / itterations;

    //Calculate the bias as a percentage
    L1_bias = (float)bechmarkM / (float)averageValL1;
    L2_bias = (float)bechmarkM / (float)averageValL2;
    R1_bias = (float)bechmarkM / (float)averageValR1;
    R2_bias = (float)bechmarkM / (float)averageValR2;

    baseLineValue = (getCalSensorVal(L1) + getCalSensorVal(L2) + getCalSensorVal(R1) + getCalSensorVal(R2)) / 4; //Baseline value of left and right sensors

    if (baseLineValue == 1023)
    {
        Serial.println("The sensors are saturated. If you are in an area of high IR activity either shield the robot or use different value resistors on SB-002");
        speaker_on();
        delay(5000);
        speaker_off();
        executeBTcommand('O');
    }

    // 2 beeps to indicate that cal has finished
    speaker_on();
    delay(450);
    speaker_off();
    delay(200);
    speaker_on();
    delay(450);
    speaker_off();
    Serial.print("Sensors calibrated, baseline value:  ");
    Serial.println(baseLineValue);

    isCald = true;
    // Delay to allow user to put robot over the line
    delay(500);
}

void test_proc()
{ 
    delay(20);
    Serial.print(getRawSensorVal(L1));
    Serial.print(",");
    Serial.print(getRawSensorVal(L2));
    Serial.print(",");
    Serial.print(getRawSensorVal(M));
    Serial.print(",");
    Serial.print(getRawSensorVal(R2));
    Serial.print(",");
    Serial.println(getRawSensorVal(R1));
    
}

In the above code I use a function called LFHandler(mode, fullTele), this function is used to detect a line; if a line is present the selected algorithm will iterate otherwise the robot will do something else. In this case the robot will move backwards. This will allow us to evaluate each algorithm separately knowing that the robot will behave consistently.

Algorithm 1 (Mode 6) - A Simple Method

This algorithm will compare the readings from the middle left and middle right sensors, it will then compare the values and if the difference is more than 20% of each value the robot will either turn left or right at a constant speed. The steps are as follows;

  1. Get sensor values from the left and right sensors
  2. Calculate the 20% tolerance
  3. If the left and right values are within 20% of each other go forwards at speed 100
  4. Otherwise if the left value is greater than the right value turn right at speed 50
  5. Otherwise if the right value is greater than the left value turn left at speed 50

The 20% figure was used to make the robot less jerky and to ensure that it behaved as expected (due to the program loop taking less than a millisecond to execute). This is the simplest line following algorithm that we can make and as a result it is very slow and jerky. The robot will gyrate from left to right as it over corrects itself to try and stay on the line. You can change the values to see how it affects the robots behaviour. For instance you can change the speed of the turns, the forwards speed and the tolerance to see what happens.

The below video shows MIPR operating in this mode;

Algorithm 2 (Mode 7) - An Intermediate Method

The second algorithm is similar to the first however it is much smoother and faster. With this algorithm we take readings from the 2 left and 2 right sensors, we then average the reading and calculate an error value. This error will be the difference between the left average value and the right average value. We then look to see if this error is a negative number or a positive number.  We then drive to robot forward using the error values for the motors speed. Either adding the error to the base speed or taking the error away from the base speed depending on which way we want the robot to turn.

For negative error values we want it to turn left and for positive values we want it to turn right. Before using these values we check to make sure that they are between the min and max limits. Once this is done we check to see if the errors are within a certain tolerance of each other in this case it's 1.8 which equates to 80%. If they are we go forward otherwise we turn sharply to the left or right depending on the sign of the number.

The algorithms steps are shown below;

  1. Get sensor values from the left and right sensors
  2. Calculate the average for left and right sensors
  3. Calculate the motor speed using the error values, base speed and sign of the error
  4. Check to ensure that the speed is in between min and max value limits -> if not set the values to the min/max values
  5. Check for a sharp turn -> if so turn left or right depending on the error values sign
  6. Otherwise go forward with new speed values

The below video shows MIPR operating in this mode;

Algorithm 3 (Mode 8) - The Golden Standard a PID Algorithm

INSERT EXPLAN HERE