My Arduino Line Following Robot !!! - With Pololu QTR-6A IR Reflectance Sensor Array

I used an Arduino Duemillanove with the ATMega 328. Propulsion is provided by the two Parallax Futaba Continuous Rotation Servos. My sensor is the Pololu QTR-6A IR Reflectance Sensor Array, and it is all powered off 4 rechargeable NiMH Duracell AA Batteries :)

It can follow a dark like, on a light background. In this case i used black tape on a whiteboard. It first calibrates itself for 5 seconds. You move it across the line a few times so it gets used to the difference in reflectance. After the calibration it begins moving foward. I used an algorithm to determine its error off the line. If it determines through the algorithm that it is at an extreme error, it will turn for a longer amount of time. Similarly, if the robot determines it is only a fraction of an inch off the line, it will only turn for a fraction of a second. This reduces over compensation and makes the line following a little smoother and more reliable.

This is the code I used, I started it from scratch and added the MegaServo Library. I'm aware that there is a library for the Pololu IR sensor arrays, but I encountered problems so i decided to start from scratch with the sensor reading as well. I'm using the Analog version of the Pololu sensor array, as opposed to the RC version, which outputs a digital signal. My sensors output an Analog voltage based on the reflectance of the surface. For example, if you are providing 5V to the sensors at Vcc, and you encounter a dark surface, that sensor will output a a voltage closer to 5V. Conversely, if the sensor encounters a very reflective, (white surface) it will output closer to 0V. I can read these 6 Analog outputs from my 6 sensors through the 6 Analog I/O pins on my Arduino.
In addition, my STOP algorithm uses nested if statements to check 3 times if the robot is really at the end, before it stops for 10 seconds, blinking the light. This prevents an accidental stop in the middle of the track due to inaccurate readings or glitches. During calibration, I calculated an average value of reflectance which i use later on to help with the navigation and decision making. I also printed some data to the Serial screen for testing purposes.

Here's my code:

// ==========================================================
// Feel free to change and use this code, but please give me credit.
// Author: Austin Duff - June 24, 2009
// ==========================================================
#include <PololuQTRSensors.h>
#include <Servo.h>
#include <MegaServo.h>
#define NBR_SERVOS 3
#define FIRST_SERVO_PIN 2

Servo left;
Servo right;
Servo tower;

MegaServo Servos[NBR_SERVOS] ;

int pingPin = 7;
int mid = 0;
int mn = 0;
int mx = 0;

void setup()
{

Servos[0].attach(2, 800, 2200); //tower
Servos[1].attach(9, 800, 2200); //left
Servos[2].attach(10, 800, 2200); //right
Serial.begin(9600);
Servos[0].write(65);
digitalWrite(13, LOW);

Servos[2].write(90);
Servos[1].write(90);

for(int i=0; i<5000; i++)
{
digitalWrite(13, HIGH);

int val = 0;
for(int j=0; j<=5; j++)
{
val = analogRead(j);
if(val >= mx)
mx = val;
if(val <= mn)
mn = val;
}
delay(1);
}

mid = ((mx + mn)/2);
digitalWrite(13, LOW);

Servos[2].write(90);
Servos[1].write(90);
}
void loop()
{

int s0 = 0;
int s1 = 0;
int s2 = 0;
int s3 = 0;
int s4 = 0;
int s5 = 0;

s0 = analogRead(0);
s1 = analogRead(1);
s2 = analogRead(2);
s3 = analogRead(3);
s4 = analogRead(4);
s5 = analogRead(5);

Serial.print("Mid: ");
Serial.print(mid);
Serial.print(" ");
Serial.print(s0);
Serial.print(" ");
Serial.print(s1);
Serial.print(" ");
Serial.print(s2);
Serial.print(" ");
Serial.print(s3);
Serial.print(" ");
Serial.print(s4);
Serial.print(" ");
Serial.print(s5);
Serial.print(" ");
Serial.println();

Servos[2].write(180);
Servos[1].write(0);

delay(10);

if((((s0+s1+s2)/3)>(((s3+s4+s5)/3)+250))&&(!((s0 > mid)&&(s5 > mid))))
{
Servos[2].write(180);
Servos[1].write(90);
Serial.print(" RIGHT");
delay(abs((((s5+s4+s3)/3)-((s0+s1+s2)/3))/2));
}
if((((s0+s1+s2)/3)<(((s3+s4+s5)/3)-250))&&(!((s0 > mid)&&(s5 > mid))))
{
Servos[2].write(90);
Servos[1].write(0);
Serial.print(" LEFT");
delay(abs((((s5+s4+s3)/3)-((s0+s1+s2)/3))/2));
}
if((s0 > mid)&&(s5 > mid))
{
Servos[2].write(90);
Servos[1].write(90);
Serial.print(" STOP");
if((s0 > mid)&&(s5 > mid))
{
Servos[2].write(90);
Servos[1].write(90);
Serial.print(" STOP");
if((s0 > mid)&&(s5 > mid))
{
Servos[2].write(90);
Servos[1].write(90);
Serial.print(" STOP");
for(int k=0; k<50; k++)
{
digitalWrite(13, HIGH);
delay(100);
digitalWrite(13, LOW);
delay(100);
}
delay(5000);
}
}
}

}