Software and Coding

#include <Wire.h>
int IRsensorAddress	= 0xB0;
int slaveAddress;

byte data_buf[16];
int i;

int Ix[4];
int Iy[4];
int s;

boolean left 		= LOW;
boolean right 		= LOW;
//boolean forward 	= LOW;
boolean reverse		= LOW;
boolean ledState	= LOW;

int forwardPWM  = 0;
//int reversePWM  = 0;
int steerValue  = 0;

const long PERIOD	= 5; 	// 50Hz Main Loop - Milliseconds
const long TIMEOUT 	= 350; 	// Action persistence - Milliseconds
const int STEERWIDE	= 128;	// Steering Width
const byte MAXSIZE	= 0xE4; // Wii Blob Max Size
const byte GAIN		= 0x28; // Wii Gain (Inverse: Smaller = higher)
int backValue 		= 90; 	// Reverse ADC Reading, calibrated away during startup
const int REVCAL	= 64;	// Shift factor added to backValue calibration

const int LEDPIN	= 13;
const int FWDPIN	= 6;
const int BACKPIN	= 9;
const int LEFTPIN	= 10;
const int RIGHTPIN	= 11;

const int REVPIN 	= A0;

long currentTime	= 0;
long prevTime 		= 0;
long cycleTime		= 0;

void setup(){
	slaveAddress = IRsensorAddress >> 1;   // This results in 0x21 as the address to pass to TWI
    Serial.begin(38400);
    pinMode(LEDPIN, OUTPUT);
	pinMode(FWDPIN, OUTPUT);
	pinMode(BACKPIN, OUTPUT);
	pinMode(LEFTPIN, OUTPUT);
	pinMode(RIGHTPIN, OUTPUT);
	
    Wire.begin();
    initiateCamera(MAXSIZE,GAIN);
    //initiateCamera(byte(analogRead()/4),byte(analogRead()/4));
    //delay(650);
    backValue= (analogRead(REVPIN)+REVCAL);
}

void loop(){
	currentTime = millis();
	sampleCamera();
	
	//printDebug();
	//Serial.println(analogRead(REVPIN));
        //Serial.println(forwardPWM);
	//Serial.println(steerValue);
        //Serial.println(cycleTime);
	
	if(Iy[0]==1023){
		if(currentTime - prevTime > TIMEOUT){
			steerValue=0;
			if(forwardPWM>0) forwardPWM--;
			else forwardPWM = 0;
			if(analogRead(REVPIN)>backValue){
				reverse = HIGH;
			}
			else{
				reverse = LOW;
			}
		}
	}
	else{
		prevTime = currentTime;
		if(forwardPWM<((1020-Iy[0])/4)) forwardPWM++;
		else forwardPWM--;
		//forwardPWM=(1023-Iy[0])/4;
		reverse = LOW;
		steerValue = (Ix[0]-511)/2;
	}

//	 if(steerValue<0){
//		 steerValue = -steerValue;
//		 analogWrite(LEFTPIN,0);
//		 analogWrite(RIGHTPIN,steerValue);
//	 }
//	 else{
//		 analogWrite(LEFTPIN,steerValue);
//		 analogWrite(RIGHTPIN,0);
//	 }

	if(steerValue < -STEERWIDE){
		digitalWrite(LEFTPIN,LOW);
		digitalWrite(RIGHTPIN,HIGH);
	}
	else if(steerValue > STEERWIDE){
		digitalWrite(LEFTPIN,HIGH);
		digitalWrite(RIGHTPIN,LOW);
	}
	else{
		digitalWrite(LEFTPIN,LOW);
		digitalWrite(RIGHTPIN,LOW);
	}

	analogWrite(FWDPIN,forwardPWM);
	digitalWrite(BACKPIN,reverse);

	ledState = !ledState;
	digitalWrite(LEDPIN, ledState);

	cycleTime = millis()-currentTime;					// Comment these lines to remove RTOS-like main loop
        //while(millis() - currentTime < PERIOD);
}

void initiateCamera(byte size, byte gain){
	Write_2bytes(0x30,0x01); delay(10);		//Control byte, allows modification of settings
	Write_2bytes(0x06,size); delay(10);		// 10 MAXSIZE - Maximum blob size. Wii uses values from 0x62 to 0xc8.
	Write_2bytes(0x08,gain); delay(10);		// 15 GAIN - Sensor Gain. Smaller values = higher gain. Numerical gain is proportional to 1/2^(n/16) for n<0x40
	Write_2bytes(0x1A,byte(gain-1)); delay(10);	// 10 GAINLIMIT - Sensor Gain Limit. Must be less than GAIN for camera to function. No other effect?
	//Write_2bytes(0x1B,0x05); delay(10);	// * MINSIZE - Minimum blob size. Wii uses values from 3 to 5
	Write_2bytes(0x33,0x33); delay(10);		// 
	Write_2bytes(0x30,0x08); delay(10);		//Was Out of order, needs to be at end
	delay(100);
}

void sampleCamera(){
	Wire.beginTransmission(slaveAddress);
    Wire.send(0x36);
    Wire.endTransmission();
	
	Wire.requestFrom(slaveAddress, 16);        // Request the 2 byte heading (MSB comes first)
    for (i=0;i<16;i++) { data_buf[i]=0; }
    i=0;
    while(Wire.available() && i < 16) { 
        data_buf[i] = Wire.receive();
        i++;
    }

    Ix[0] = data_buf[1];
    Iy[0] = data_buf[2];
    s   = data_buf[3];
    Ix[0] += (s & 0x30) <<4;
    Iy[0] += (s & 0xC0) <<2;

    Ix[1] = data_buf[4];
    Iy[1] = data_buf[5];
    s   = data_buf[6];
    Ix[1] += (s & 0x30) <<4;
    Iy[1] += (s & 0xC0) <<2;

    Ix[2] = data_buf[7];
    Iy[2] = data_buf[8];
    s   = data_buf[9];
    Ix[2] += (s & 0x30) <<4;
    Iy[2] += (s & 0xC0) <<2;

    Ix[3] = data_buf[10];
    Iy[3] = data_buf[11];
    s   = data_buf[12];
    Ix[3] += (s & 0x30) <<4;
    Iy[3] += (s & 0xC0) <<2;
}

void printDebug(){
	for(i=0; i<4; i++){
		if (Ix[i] < 1000)	Serial.print(" ");
		if (Ix[i] < 100)	Serial.print(" ");
		if (Ix[i] < 10)		Serial.print(" ");
		Serial.print( int(Ix[i]) );
		Serial.print(",");
		if (Iy[i] < 1000)	Serial.print(" ");
		if (Iy[i] < 100)	Serial.print(" ");
		if (Iy[i] < 10)		Serial.print(" ");
		Serial.print( int(Iy[i]) );
		if (i<3)			Serial.print(",");
	}
    Serial.println("");
    delay(3);
}

void Write_2bytes(byte d1, byte d2){
	Wire.beginTransmission(slaveAddress);
	Wire.send(d1); Wire.send(d2);
	Wire.endTransmission();
}