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.

67 comments:

  1. Fantastic! Put it together, and works a charm. Now to get some processing sketch to display both. I've modified WiiChuck.h to 'switch-on' the nunchuck in the new way - tomorrow i'll add the orig wm+ code to it...

    P.s... glad you found the youtube video - It was for a few other people to show them progress on my little project, so apologies for no code-attribution! :)

    - Adrian
    (Can't wait for the Kalman... and I'd love to hear tips on how it can be utilised with GPS data...)

    ReplyDelete
    Replies
    1. If some one needs to be updated with hottest technologies then he must be pay a quick visit this web page and be up to date everyday.

      Delete
  2. Great work. Looking foward to Kalman filter.
    Your circuit works great. Now to integrate it into the ArduPilot instead of those expensive IR sensors.
    Earl

    ReplyDelete
  3. The comment from anomymous above was me...
    Signed up for acct here.
    Earl

    ReplyDelete
  4. Hey knuckles - check out my first cut of Kalman code... http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/0

    I've taken Jordi's Kalman code from ArduPilot, configured it to support the WM+ and rolled it back into the code.

    I think it still needs some tweaking, but its a great start! :) Also turned off nunchuck encryption to save XOR etc.

    Thanks heaps for making this information public in the first place. Now to get the other 2 axis incorporated so we have a cheap-as-chips 6DOF IMU...

    ReplyDelete
  5. The calibrateZeroes method has a slight bug that will affect gyro calibration. The averaging (division by 10) needs to happen after the end of the outer loop. Because of possible numerical overflow, I created 3 local long variables to accumulate the data, then the result of the division was stored in your yaw0, roll0, and pitch0 as ints.

    Duck

    ReplyDelete
  6. Hello, how are you?.
    Anyone know if this code works with mega arduino.
    Because I do not work and it is why.
    Thanks
    PS: I feel that my English is bad

    ReplyDelete
  7. I am wondering if anyone knows if/how you could use this setup with Max msp - that is: all six degrees of freedom

    ReplyDelete
  8. Thanks for all of your hard work - this is just great!

    ReplyDelete
  9. Thanks for this article, it is really helpful to complete my task.

    ReplyDelete
  10. This is the perfect blog for anyone who wants to know about this topic. You know so much its almost hard to argue with you (not that I really would want...HaHa). You definitely put a new spin on a subject thats been written about for years. Great stuff, just great!

    Stop Korupsi Dan Suap Di Indonesia | Stop Korupsi Dan Suap Di Indonesia | CARA BERIKLAN DI INTERNET | Stop Korupsi Dan Suap Di Indonesia | Stop Korupsi Dan Suap Di Indonesia | Stop Korupsi Dan Suap Di Indonesia | Stop Korupsi Dan Suap Di Indonesia | Info Kontes SEO Stop Korupsi Dan Suap Di Indonesia

    ReplyDelete
  11. your post looks very interesting for me, Great blog! I am loving it!! Will come back again. I am totally excited about this blog
    foto gadis smu, camera,komputer game, film, Pencairan Es Greenland

    ReplyDelete
  12. your post looks very interesting for me, Great blog! I am loving it!! Will come back again. I am totally excited about this blog
    foto gadis smu,camera,komputer game,film,Pencairan Es Greenland

    ReplyDelete
  13. Thank you for another essential article. Where else could anyone get that kind of information in such a complete way of writing? I have a presentation incoming week, and I am on the lookout for such information.

    Arsenal vs Huddersfield Town live streaming | Lazio vs Fiorentina live streaming | Everton vs Chelsea live Streaming | Technology News | News Today | Live Streaming

    ReplyDelete
  14. This is the perfect blog for anyone who wants to know about this topic. You know so much its almost hard to argue with you (not that I really would want...HaHa). You definitely put a new spin on a subject thats been written about for years. Great stuff, just great!

    cara beriklan di internet | cara beriklan di internet | cara beriklan di internet | cara beriklan di internet | CARA BERIKLAN DI INTERNET | cara beriklan di internet | CARA BERIKLAN DI INTERNET | cara beriklan di internet

    ReplyDelete
  15. great finding but what will this this do?

    ReplyDelete
  16. Hey miles.. Can I use your http://i39.tinypic.com/1zxryax.jpg image on Make Projects... I am using your code as the Arduino code for this project that I am putting together. email me if you don't mind.. I attribute everything to you of course.

    riley nospace porter at gmail . com

    ReplyDelete
  17. erg
    buen trabajo y buena cabeza

    ReplyDelete
  18. Después de leer tanto talento junto, me atrevo a hacer una pregunta: se puede con un magnetrometro Y un acelerómetro de tres ejes medir el rool y el pich en movimiento?.Gracias

    ReplyDelete
  19. I was very encouraged to find this site. I wanted to thank you for this special read. I definitely savored every little bit of it and I have you bookmarked to check out new stuff you post.

    ReplyDelete
  20. Thank you for another essential article. Where else could anyone get that kind of information in such a complete way of writing? I have a presentation incoming week, and I am on the lookout for such information.

    ReplyDelete
  21. glad you found the youtube video - It was for a few other people to show them progress on my little project, so apologies for no code-attribution! :)

    ReplyDelete
  22. I am thoroughly convinced in this said post. I am currently searching for ways in which I could enhance my knowledge in this said topic you have posted here. It does help me a lot knowing that you have shared this information here freely. I love the way the people here interact and shared their opinions too. I would love to track your future posts pertaining to the said topic we are able to read.

    ReplyDelete
  23. Hello,
    This is great information....... Thanks very much for the share.... Keep posting.

    Thanks!

    ReplyDelete
  24. we offer discount nike jordan shoes up to 30%-50% off, nike jordan shoes is a kind of street style shoes online store, they are all the newest and discount nike jordan shoes.
    nike air max 2011
    Nike air max 2011 nike shox torch cheap air jordan 2010
    air jordan 2010
    jordan retro shoes
    new jordan shoes 2010

    ReplyDelete
  25. The author has written an excellent article. You have made your point and there is not much to argue about. It is like the following universal truth that you can not argue with: No truth is universal, everything has its exception. Thanks for the info penis enlargement penis enlargement pills VigRX Plus

    ReplyDelete
  26. That's a very good post! I have learnt so much from this post! And I will tell my friends this website. Thanx for your sharing.pf changs coupons

    ReplyDelete
  27. This is the perfect blog for anyone who wants to know about this topic. You know so much its almost hard to argue with you (not that I really would want...HaHa). You definitely put a new spin on a subject thats been written about for years. Great stuff, just great! mid autumn festival 2011 holiday forum Singapore travel chinese new year 2012 Thailand travel

    ReplyDelete
  28. Thanks for sharing these lovely information...

    ReplyDelete
  29. sometimes that is not important and we ignore will become very important and meaningful.

    ReplyDelete
  30. Thanks for providing such a great information here.
    Actually,I was looking forward to such a info.I really appreciate it.Hope to see such a informative stuff in future!

    ReplyDelete
  31. Such a nice and interesting post. Keep it up.

    ReplyDelete
  32. Hrmm that was weird, my comment got eaten. Anyway I wanted to say that it's nice to know that someone else also mentioned this as I had trouble finding the same info elsewhere. This was the first place that told me the answer. Thanks.
    Australia Public Holidays 2012 Calendar
    Calendar 2012
    Singapore Public Holidays 2012 Calendar
    Thanksgiving 2011

    ReplyDelete
  33. Well thanks I don't use icons but I like to collect pictures of any kind.
    Generic Viagra 100mg

    ReplyDelete
  34. Thank you for another informative web site. The place else may I am getting that kind of information written in such an ideal method? I have a project that I am simply now running on, and I have been at the glance out for such information.

    ReplyDelete
  35. Hey I'm working on doing this without the extra hardware or adding long delays due to disabling the devices. If your interested in helping with software message me reibot.org

    ReplyDelete
  36. This is the perfect blog for anyone who wants to know about this topic. You know so much its almost hard to argue with you (not that I really would want...HaHa). You definitely put a new spin on a subject thats been written about for years. Great stuff, just great! Sbobet Ibcbet Bola Tangkas Casino Sbobet Piala Eropa 2012 Score Bola

    ReplyDelete
  37. Sesli Sohbet , Sesli Chat , Kameralı Sohbet , Arkadaşlık Sitesi
    Site Hakkında: Sesli Sohbet, Sesli Chat, Kameralı Sohbet Yapabileceğiniz Ücretsiz Sosyal Paylaşım Sitesidir.

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

    ReplyDelete
  39. Grazie per un altro sito web informativo. L'altro posto può Ricevo questo tipo di informazioni scritte in un metodo ideale? Ho un progetto che sto semplicemente ora in esecuzione, e io sono stato al colpo d'occhio per tali informazioni.
    Bisnis Online

    ReplyDelete
  40. Thanks a lot for you article, this is a very useful article for me, thanks !
    Pelatihan Motivasi

    ReplyDelete
  41. Dank u voor een andere informatieve website. De plaats anders kan Ik krijg dat soort informatie geschreven in een dergelijke ideale methode? Ik heb een project dat ik gewoon ben nu draait op, en ik ben al bij de blik uit voor dergelijke informatie.

    ReplyDelete
  42. Hey I'm working on doing this without the extra hardware or adding long delays due to disabling the devices. If your interested in helping with software message me reibot.org
    cheap football jerseys
    jerseys cheap
    cheap nfl jerseys

    ReplyDelete
  43. Questo è il blog ideale per chi vuole conoscere su questo argomento. Sai così tanto la sua quasi difficile discutere con te (non che io davvero vorrei ... haha). È sicuramente messo una nuova rotazione su un tema thats scritto per anni. Grandi cose, semplicemente fantastico!

    ReplyDelete
  44. This is the perfect blog for anyone who wants to know about this topic. You know so much its almost hard to argue with you (not that I really would want...HaHa). You definitely put a new spin on a subject thats been written about for years. Great stuff, just great! Agen Sbobet Agen Ibcbet

    ReplyDelete
  45. What youre saying is completely true. I know that everybody must say the same thing, but I just think that you put it in a way that everyone can understand. I also love the images you put in here. They fit so well with what youre trying to say. Im sure youll reach so many people with what youve got to say. Agen Ibcbet Bandar Bola Tangkasnet

    ReplyDelete
  46. I must admit that this is one great insight. It surely gives a company the opportunity to get in on the ground floor and really take part in creating something special and tailored to their needs. Bandar Bola Judi Online Judi Bola

    ReplyDelete
  47. Mate this is a very nice blog here. I wanted to comment & say that I enjoyed reading your posts & they are all very well written out. You make blogging look easy lol I’ll attemp to start a blog later today and I hope it’s half as good as your blog! Much success to you! Judi Bola Online

    ReplyDelete
  48. Would this work with Arduino Nano 3.0?

    ReplyDelete
  49. Thanks have been given the opportunity to comment here, I Judi Bola very happy with what you write.

    ReplyDelete