Motion Plus and Nunchuck together on the Arduino

Easy as 1-2-3

Alright, one great aspect about the Wii Motion Plus is its pass through port for other extension controllers such as the nunchuck. Unfortunately, no one has been able to read both an active motion plus and any other controller at the same time because they are all on the same I2C address(smooth nintendo). This is creates a large hurdle to people like myself who bought the WM+ to create a low cost IMU with it and the accelerometer in the nunchuck. After a good deal of digging and very little luck, I did find a way to use both at the same time(though not through the pass through port). And best of all its cheap!

Note before continuing: Please remember that I am disclosing this method free of cost in the spirit of open sourciness. As such, if you read this and it helps you, please help me a little by clicking on one(or more) of the ads to the side. It will help me out(monetarily) and let me feel like Ive gotten a little back for publishing it instead of keeping it to myself. Dont worry, they wont bite (theyre google ads).

Credit where credit is due: I got the idea from this post, so many thanks johnnyonthespot

Materials:
2 NPN type switching transistors (I used 2N2222-super easy to find)
2 current limiting resistors (I used 470 Ohm)
Breadboard and jumper wires or make your own board

All of these materials can be easily and very cheaply bought at radioshack, mouser, digikey, or the like.
Literally, it will cost like $4.00 if you already have a breadboard

Rigging it Up:
Here is a simple schematic for breadboarding:


Code:
#include <Wire.h>
#include <Streaming.h>

//WM+ stuff
byte data[6]; //six data bytes
int yaw, pitch, roll; //three axes
int yaw0, pitch0, roll0; //calibration zeroes

//nunchuck stuff
uint8_t outbuf[6]; // array to store arduino output
int cnt = 0;
int joy_x_axis, joy_y_axis, accel_x_axis, accel_y_axis, accel_z_axis;
boolean z_button, c_button;

void setup(){
Serial.begin(115200);
Serial << "WM+ and Nunchuck tester" << endl;
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
digitalWrite(3, HIGH);
digitalWrite(4, HIGH);
Wire.begin();
switchtowmp();
wmpOn();
calibrateZeroes();
switchtonunchuck();
nunchuck_init();
}

void loop(){
switchtowmp();
receiveData();
//receiveRaw();
switchtonunchuck();
receive_nunchuck_data();
Serial << yaw << "\t" << pitch << "\t" << roll << "\t" << joy_x_axis << "\t" << joy_y_axis << "\t";
Serial << accel_x_axis << "\t" << accel_y_axis << "\t" << accel_z_axis;
Serial << "\t" << _DEC(z_button) << "\t" << _DEC(c_button) << endl;
delay(100);
}

void nunchuck_init ()
{
Wire.beginTransmission (0x52);// transmit to device 0x52
Wire.send (0x40); // sends memory address
Wire.send (0x00); // sends sent a zero.
Wire.endTransmission (); // stop transmitting
}

void send_zero ()
{
Wire.beginTransmission (0x52);// transmit to device 0x52
Wire.send (0x00); // sends one byte
Wire.endTransmission (); // stop transmitting
}

void receive_nunchuck_data(){
Wire.requestFrom(0x52, 6);
for (int i=0;i<6;i++){
data[i]=Wire.receive();
}
make_nunchuck_data();
send_zero();
}

void make_nunchuck_data ()
{
for(int i=0;i<6;i++){
outbuf[i]=nunchuck_decode_byte(data[i]);
}
joy_x_axis = outbuf[0];
joy_y_axis = outbuf[1];
accel_x_axis = outbuf[2] * 2 * 2;
accel_y_axis = outbuf[3] * 2 * 2;
accel_z_axis = outbuf[4] * 2 * 2;
z_button = 0;
c_button = 0;

// byte outbuf[5] contains bits for z and c buttons
// it also contains the least significant bits for the accelerometer data
// so we have to check each bit of byte outbuf[5]
if ((outbuf[5] >> 0) & 1)
{
z_button = 1;
}
if ((outbuf[5] >> 1) & 1)
{
c_button = 1;
}

if ((outbuf[5] >> 2) & 1)
{
accel_x_axis += 2;
}
if ((outbuf[5] >> 3) & 1)
{
accel_x_axis += 1;
}

if ((outbuf[5] >> 4) & 1)
{
accel_y_axis += 2;
}
if ((outbuf[5] >> 5) & 1)
{
accel_y_axis += 1;
}

if ((outbuf[5] >> 6) & 1)
{
accel_z_axis += 2;
}
if ((outbuf[5] >> 7) & 1)
{
accel_z_axis += 1;
}
}

char nunchuck_decode_byte (char x)
{
x = (x ^ 0x17) + 0x17;
return x;
}

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 wmpOff(){
Wire.beginTransmission(82);
Wire.send(0xf0);//address then
Wire.send(0x55);//command
//Wire.send(0x00);
//Wire.send(0xfb);
Wire.endTransmission();
}

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;// for each zero
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();
}
//see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
//for info on what each byte represents
yaw=((data[3] >> 2) << 8)+data[0]-yaw0;
pitch=((data[4] >> 2) << 8)+data[1]-pitch0;
roll=((data[5] >> 2) << 8)+data[2]-roll0;
}

void receiveRaw(){
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];
pitch=((data[4] >> 2) << 8)+data[1];
roll=((data[5] >> 2) << 8)+data[2];
}

void switchtonunchuck(){
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(4, HIGH);
}

void switchtowmp(){
digitalWrite(3, LOW);
digitalWrite(4, LOW);
digitalWrite(3, HIGH);
}

Closing Comments
If some of this code looks wierd to you its probably the streaming operator(<<). I prefer to use this form from mikalhart's streaming library. If you dont like it or dont want to download it, just replace any << with a .print() of the type specified before the << operator (ex: Serial << "hi" << endl; is the same as Serial.print("hi"); Serial.println(); )

Also remember to reference the definitions of the axes for each sensor; yaw, pitch, and roll are not the same for both, nor are they necessarily what I would have picked them to be. Im just keeping with what prior work has named them. Also, if you want a full fledged IMU from these components, stay tuned. I should have a decent kalman filtered IMU demo coming soon. Woot!

Lastly, I find the Arduino.cc forum to be much better at handling questions so try to reach me there on my post in the exhibition section if your comment here doesnt get answered.

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!