Wii Motion Plus + Arduino = Love

Ok so I, after much research, have been able to read the gyro data of the new wii motion plus peripheral with the arduino microcontroller. With this code and the code previously developed for the wii nunchuck (here), we are able to create a 6 DOF IMU for under $40. Thanks Nintendo! Best of all, everything is I2C so only 2 analog inputs (A4 and A5 needed for the wire library) are needed to read 6 sensors and no ADC conversion happens on the arduino board.

Links and Thanks:
First off, I'm going to link(and thank) a few sites that have helped me immensely with this undertaking:
Arduino.cc
original nunchuck code
Wii brew WM+ info
Propeller forums

Hardware connections:
I used full 5V TTL signals and power and have had no problems thus far (a week going strong) but if you have a 3.3v arduino or a level converter and 3.3v regulator, i would suggest that over a 5v connection. Furthermore, twi.h under the wire library does not need to be changed for my setup but might under some setups. Start with the default 100khz TWI_FREQ and if that doesnt work, use 400khz. Websites disagree about which is the proper speed for direct I2C on wii peripherals. Connections to WM+ are same as nunchuck and look like this:

| 1 2 3 |
|       |
| 6 5 4 |
|_-----_|

1 - green - data
2 - nothing
3 - red - 3.3+v
4 - yellow - clock
5 - nothing
6 - white - ground

So its pin 3 to 5v, 6 to ground, 1 to A4, and 4 to A5

Adapters such as the one sold here should (theoretically) work; I use jumper wires and hot glue

Software:

I have commented the demo code pretty well so it should be easy to follow, if not comment here or on Arduino.cc Forums->Exhibition

#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;
}
Serial.print("Yaw0:");
Serial.print(yaw0);
Serial.print(" Pitch0:");
Serial.print(pitch0);
Serial.print(" Roll0:");
Serial.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(){
Serial.begin(115200);
Serial.println("WM+ tester");
Wire.begin();
wmpOn(); //turn WM+ on
calibrateZeroes(); //calibrate zeroes
delay(1000);
}

void loop(){
receiveData(); //receive data and calculate yaw pitch and roll
Serial.print("yaw:");//see diagram on randomhacksofboredom.blogspot.com
Serial.print(yaw); //for info on which axis is which
Serial.print(" pitch:");
Serial.print(pitch);
Serial.print(" roll:");
Serial.println(roll);
delay(100);
}

Orientation:

Sorry for the arrows being all in the negative direction. Picture was done quickly.

ENJOY!