Follow the leader: robot style

I’ve been playing with the WiiCamera the last days. After I had successfully written a sketch to track an IR light source I put the WiiCamera on my small Ebay-bot. The goal was that the robot would follow the IR light source. It was quite easy to program. I tried out my sketch by holding an IR led in front of the robot. It tracks the IR led pretty well. See the following video:

Then I decided to mount an IR led on my big Rover 5, I put it on the lower bracket of the gripper. Now the little robot could follow the Rover 5. The Rover 5 is remote controlled. I made a couple of changes to my previous sketch. When the WiiCamera loses track of the IR light source it starts sweeping the pan servo. That way it has a better chance to keep track of the IR light source. After some tuning I came up with this:

I had lots of fun with this, I’ve been driving around the house with my robots all day. I have another WiiCamera, maybe I should stick it on one of my other robots?
I really like the WiiCamera sensor, its very easy to work with. It doesn’t require difficult coding to do cool things with it. Here is the code I used to follow the Rover 5:

// http://wwww.bajdi.com
// Robot with 2 motors controlled by L293D, mcu = ATmega1284P-PU
// WiiCamera tracking IR light source (IR led, lighter, candle)
// WiiCamera mounted on pan/tilt kit, 2 SG90 servos
// Robot will try to follow the IR light source
// When it does not detect an IR led it will sweep the pan servo

// WiiCamera data:
// blob X = 0 IR source on the right
// blob X = 1024 IR source on the left
// blob Y = 0 IR source top
// blob Y = 1024 IR source bottom

#include <Servo.h>
#include <Wire.h>
#include <PVision.h>

Servo panServo;  
Servo tiltServo;
PVision ircam;

int panServoLeft = 2250;
int panServoCenter = 1550;
int panServoRight = 850;
int panServoPos;
int tiltServoUp = 2200;
int tiltServoCenter = 1550;
int tiltServoDown = 1200;
int tiltServoPos;
int servoDirection;
bool IRdetected = false;

byte result;   // data from WiiCamera

#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))

const int EN1 = 3;        //pwm, enable motor 1 = pin 1 of L293D
const int direction1 = 2; //direcion motor 1 = pin 2 of L293D
const int EN2 = 4;        //pwm, enable motor 2 = pin 9 of L293D
const int direction2 = 5; //direction motor 2 = pin 15 of L293D

int speedLeft = 205; //there seems to be a bit of a difference between these cheap motors :(
int speedRight = 255; //this motor needs a bit more voltage to keep up with the other



void setup() 
{ 
  Serial.begin(9600);
  pinMode(EN1, OUTPUT);  
  pinMode(direction1, OUTPUT); 
  pinMode(EN2, OUTPUT); 
  pinMode(direction2, OUTPUT);
  analogWrite(EN1, 0);
  analogWrite(EN2, 0);
  ircam.init();
  delay(50);
  panServo.attach(22);
  panServoPos = panServoCenter;  
  panServo.writeMicroseconds(panServoPos);
  tiltServo.attach(23); 
  tiltServoPos = tiltServoCenter; 
  tiltServo.writeMicroseconds(tiltServoPos);
  delay(500); 
} 

void loop() 
{ 
  runEvery(15)
  {
    result = ircam.read();      // get the results from the WiiCamera
    if (result & BLOB1)        // IR detected
    {
      IRdetected = true;      

      if (ircam.Blob1.X < 450 && panServoPos > panServoRight)
      {
        panServoPos -= 10;
      }
      if (ircam.Blob1.X > 550 && panServoPos < panServoLeft)
      {
        panServoPos += 10;
      }
      if (ircam.Blob1.X <= 550 && ircam.Blob1.X >= 500) // IR light close to center
      {
        panServoPos += 1;
      }
      if (ircam.Blob1.X < 500 && ircam.Blob1.X >= 450) // IR light close to center
      {
        panServoPos -= 1;
      }

      if (ircam.Blob1.Y < 450 && tiltServoPos < tiltServoUp)     
      {
        tiltServoPos += 10;
      }
      if (ircam.Blob1.Y > 550 && tiltServoPos > tiltServoDown)
      {
        tiltServoPos -= 10;
      }
      if (ircam.Blob1.Y <= 550 && ircam.Blob1.Y >= 500) // IR light close to center
      {
        tiltServoPos -= 1;
      }
      if (ircam.Blob1.Y < 500 && ircam.Blob1.Y >= 450) // IR light close to center
      {
        tiltServoPos += 1;
      }
    } 

    else {      // no IR detected = center the tilt servo, sweep the pan servo
      IRdetected = false;

      if(panServoPos < panServoLeft && servoDirection == 0)
      {                                  
        panServoPos += 10;                        
      } 
      if(panServoPos > panServoRight && servoDirection == 1) 
      {                                
        panServoPos -= 10;
      }    

      if (panServoPos >= panServoLeft )    
      {
        servoDirection = 1;  //changes the direction
      }
      if (panServoPos <= panServoRight )     
      {
        servoDirection = 0;  //changes the direction
      } 


      if (tiltServoPos > tiltServoCenter) // center tilt servo
      {
        tiltServoPos -= 10;
      }
      if (tiltServoPos < tiltServoCenter) // center tilt servo
      {
        tiltServoPos += 10;
      }
    } 
    panServo.writeMicroseconds(panServoPos);
    tiltServo.writeMicroseconds(tiltServoPos);
  }

  if(IRdetected == true)    // try to follow the IR source by moving the robot
  { 
    if (ircam.Blob1.Size < 3)  // IR source far from robot
    {
      if(panServoPos < panServoCenter+200 && panServoPos > panServoCenter-200)
      {
        forward();
      }
      if (panServoPos >= panServoCenter+200)
      {
        left();
      }
      if (panServoPos <= panServoCenter-200)
      {
        right();
      }
    }

    if (ircam.Blob1.Size >= 3)    // IR source close to robot
    {
      if(panServoPos < panServoCenter+200 && panServoPos > panServoCenter-200)
      {
        backward();
      }
      if (panServoPos >= panServoCenter+200)
      {
        left();
      }
      if (panServoPos <= panServoCenter-200)
      {
        right();
      }
    }
  }
  else
  {
    stop();
  }

  runEvery(300)   // checking reading of WiiCamera sensor on serial monitor
  { 
    Serial.print("BLOB1 detected. X:");
    Serial.print(ircam.Blob1.X);
    Serial.print(" Y:");
    Serial.print(ircam.Blob1.Y);
    Serial.print(" Size:");
    Serial.println(ircam.Blob1.Size);
  }
}

void forward()
{
  digitalWrite(direction1, HIGH);
  digitalWrite(direction2, HIGH);   
  analogWrite(EN1, speedLeft);
  analogWrite(EN2, speedRight);
}

void stop()
{
  digitalWrite(direction1, LOW);   
  digitalWrite(direction2, LOW);   
  analogWrite(EN1, 0);
  analogWrite(EN2, 0);
}

void backward()
{
  digitalWrite(direction1, LOW);  
  digitalWrite(direction2, LOW);   
  analogWrite(EN1, speedLeft+35);
  analogWrite(EN2, speedRight-15);
}

void left()
{
  digitalWrite(direction1, LOW);
  digitalWrite(direction2, HIGH);   
  analogWrite(EN1, speedLeft);
  analogWrite(EN2, speedRight);
}

void right()
{
  digitalWrite(direction1, HIGH);
  digitalWrite(direction2, LOW);   
  analogWrite(EN1, speedLeft);
  analogWrite(EN2, speedRight-30);
}

Some photos of the little robot: