Sponsors of Wiimoteproject.com
IR Pens for Wiimote Whiteboard
Pages: [1]
  Print  
Author Topic: Light Follower 2 axis  (Read 4243 times)
0 Members and 1 Guest are viewing this topic.
*
Karma: +0/-0
Posts: 11
Offline Offline
View Profile Email
« on: April 25, 2010, 10:08:03 AM »

I finaly put together a 2 axis light follower using servos and the Wiimote IR Camera. Here is a video, code, and a link to some pictures of the project. I basically stuffed the IR Camera into a project box with the oscillator. An onboard LED lights up when a light source is present.

<a href="http://www.youtube.com/watch?v=y41o7KieRgw" target="_blank">http://www.youtube.com/watch?v=y41o7KieRgw</a>
http://www.youtube.com/watch?v=y41o7KieRgw
http://ohmwardbond.blogspot.com/2010/04/light-follower-2-axis.html

Code:
#include <Wire.h>
#include <PVision.h>
#include <Servo.h>
 
Servo servo1;
Servo servo2;
int ledPin = 12;
PVision ircam;
byte result;

//servo 1
#define S1_LEFT_SLOW 1550
#define S1_LEFT_MEDIUM 1700
#define S1_LEFT_FAST 1800
#define S1_RIGHT_SLOW 1450
#define S1_RIGHT_MEDIUM 1300
#define S1_RIGHT_FAST 1200
#define STOP 1500
//servo 2
#define S2_RIGHT_SLOW 1510
#define S2_RIGHT_MEDIUM 1525
#define S2_RIGHT_FAST 1560
#define S2_LEFT_SLOW 1490
#define S2_LEFT_MEDIUM 1475
#define S2_LEFT_FAST 1440


int s;

void setup()
{
  Serial.begin(115200);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);
  servo1.attach(9);
  servo2.attach(10);
  ircam.init();
  digitalWrite(13, HIGH);
 
 
}
int x;
void loop()
{
  result = ircam.read();
 
 

  if (result & BLOB1)
  {
    digitalWrite(ledPin, HIGH);
//SERVO 1
      if (ircam.Blob1.X > 480 && ircam.Blob1.X < 520)
        {
          servo1.write(STOP);
        }

//Servo 1 moves left
      else if (ircam.Blob1.X < 480)
        {

        if (ircam.Blob1.X < 200)
          {
             move_servo1(S1_LEFT_FAST);
          }
        else if (ircam.Blob1.X < 400)
          {
             move_servo1(S1_LEFT_MEDIUM);
          }
        else
          {
             move_servo1(S1_LEFT_SLOW);
          }
   
        }
//servo 1 moves right
      else if (ircam.Blob1.X > 520)
        {   
         if (ircam.Blob1.X > 800)
          {
             move_servo1(S1_RIGHT_FAST);
          }
        else if (ircam.Blob1.X > 600)
          {
             move_servo1(S1_RIGHT_MEDIUM);
          }
         else
          {
             move_servo1(S1_RIGHT_SLOW);
          }

        }     
//servo 1 end

//SERVO 2
if (ircam.Blob1.Y > 365 && ircam.Blob1.Y < 405)
{
  servo2.write(STOP);
}

  else if (ircam.Blob1.Y < 365)
{

  if (ircam.Blob1.Y < 150)
    {
     move_servo2(S2_LEFT_FAST);
    }
   else if (ircam.Blob1.Y < 270)
    {
     move_servo2(S2_LEFT_MEDIUM);
    }
    else
    {
     move_servo2(S2_LEFT_SLOW);
    }
   
}

else if (ircam.Blob1.Y > 405)
{
 if (ircam.Blob1.Y > 650)
    {
     move_servo2(S2_RIGHT_FAST);
    }
    else if (ircam.Blob1.Y > 500)
    {
     move_servo2(S2_RIGHT_MEDIUM);
    }
    else
    {
     move_servo2(S2_RIGHT_SLOW);
    }

}     

  }//Ends if Blob
else
{
  digitalWrite(ledPin, LOW);
 servo1.write(STOP);
servo2.write(STOP);
}

}// ends loop

void printBLOB(int irX)
{
 Serial.println(irX);
}

void move_servo1(int val)
{
//Serial.print(val);
//Serial.print(" ");
//Serial.print(ircam.Blob1.X, DEC);
//Serial.println();
   servo1.write(val);                 

}
void move_servo2(int val)
{
//Serial.print(val);
//Serial.print(" ");
//Serial.print(ircam.Blob1.X, DEC);
//Serial.println();
   servo2.write(val);                 

}
Logged
Pages: [1]
  Print  
 
Jump to:  

Clicky Web Analytics