Sunday, July 26, 2009

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.