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.
#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);
}
