/*

http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html

Modified by Kimio kosaka

*/

#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 10, 5, 4, 3, 2);

#include <Wire.h>
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes

void wmpOn(){
	Wire.beginTransmission(0x53); //WM+ starts out deactivated at address 0x53
	Wire.send(0xfe); //send 0x04 to address 0xFE to activate WM+
	Wire.send(0x04);
	Wire.endTransmission(); //WM+ jumps to address 0x52 and is now active
}

void wmpSendZero(){
	Wire.beginTransmission(0x52); //now at address 0x52
	Wire.send(0x00); //send zero to signal we want info
	Wire.endTransmission();
}

void calibrateZeroes(){
	for (int i=0;i<10;i++){
		wmpSendZero();
		Wire.requestFrom(0x52,6);
		for (int i=0;i<6;i++){
			data[i]=Wire.receive();
		}
		yaw0+=(((data[3]>>2)<<8)+data[0])/10; //average 10 readings
		pitch0+=(((data[4]>>2)<<8)+data[1])/10;
		roll0+=(((data[5]>>2)<<8)+data[2])/10;
	}
        lcd.setCursor(0,0);
        lcd.print("WM+");
        lcd.setCursor(8,0);
	lcd.print("Y0:");
	lcd.print(yaw0);
        lcd.setCursor(0,1);
	lcd.print("P0:");
	lcd.print(pitch0);
        lcd.print("        ");
        lcd.setCursor(8,1);
	lcd.print("R0:");
	lcd.println(roll0);
}

void receiveData(){
	wmpSendZero(); //send zero before each request (same as nunchuck)
	Wire.requestFrom(0x52,6); //request the six bytes from the WM+
	for (int i=0;i<6;i++){
		data[i]=Wire.receive();
	}
	yaw=((data[3]>>2)<<8)+data[0]-yaw0;
	pitch=((data[4]>>2)<<8)+data[1]-pitch0;
	roll=((data[5]>>2)<<8)+data[2]-roll0;
}

//see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
//for info on what each byte represents

void setup(){
        lcd.begin(16,2);
        lcd.clear();
	Wire.begin();
	wmpOn(); //turn WM+ on
	calibrateZeroes(); //calibrate zeroes
	delay(1000);
}

void loop(){
	receiveData(); //receive data and calculate yaw pitch and roll
        lcd.setCursor(0,0);
        lcd.print("WM+");
        lcd.setCursor(8,0);
	lcd.print("y:");//see diagram on randomhacksofboredom.blogspot.com
	lcd.print(yaw); //for info on which axis is which
        lcd.print("        ");
        lcd.setCursor(0,1);
	lcd.print("p:");
	lcd.print(pitch);
        lcd.print("        ");
        lcd.setCursor(8,1);
        lcd.print("r:");
	lcd.print(roll);
        lcd.print("        ");
	delay(100);
}