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.

57 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. 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
  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. 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
  13. great finding but what will this this do?

    ReplyDelete
  14. 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
  15. erg
    buen trabajo y buena cabeza

    ReplyDelete
  16. 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
  17. 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
  18. 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
  19. 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
  20. 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
  21. Hello,
    This is great information....... Thanks very much for the share.... Keep posting.

    Thanks!

    ReplyDelete
  22. 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
  23. 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
  24. sometimes that is not important and we ignore will become very important and meaningful.

    ReplyDelete
  25. 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
  26. 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
  27. Well thanks I don't use icons but I like to collect pictures of any kind.
    Generic Viagra 100mg

    ReplyDelete
  28. 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
  29. 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
  30. 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
  31. http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html

    ReplyDelete
  32. 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
  33. Thanks a lot for you article, this is a very useful article for me, thanks !
    Pelatihan Motivasi

    ReplyDelete
  34. 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
  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
    cheap football jerseys
    jerseys cheap
    cheap nfl jerseys

    ReplyDelete
  36. 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
  37. 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
  38. 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
  39. Would this work with Arduino Nano 3.0?

    ReplyDelete
  40. very interesting article you created for a simple blog this.
    togel online

    ReplyDelete
  41. I like and very happy to read this article and I also like your blog very good
    togel sgp

    ReplyDelete
  42. In this section, you can discover some of the facts behind gambling myths, get explanations for the terms used in the gambling industry and understand its size and how it is regulated.
    joker deposit pulsa
    Probability is the likelihood of a specific outcome or event taking place.
    aplikasi poker uang asli untuk android
    To work this out, you divide the number of specific outcomes with the number of possible outcomes.

    ReplyDelete
  43. Hai, saya pikir saya melihat Anda mengunjungi weblog saya jadi saya datang untuk "membalas budi". Saya mencoba menemukan hal-hal untuk meningkatkan situs web saya! Saya kira boleh saja menggunakan beberapa ide Anda !!
    http://www.daftarkantorkedutaan.web.id

    ReplyDelete
  44. A lot of blogs I see these days don't really provide anything that I'm interested in, but I'm most definately interested in this one.
    DAFTAR SBOBET88 ASIA

    ReplyDelete
  45. Hi there. Very cool site!! Guy ..Wonderful .. I will bookmark your website and take the feeds additionally…I am glad to locate so much useful info right here in the article. Thanks for sharing…
    http://www.tembakcacing.asia/

    ReplyDelete
  46. Saya sangat terkesan dengan bakat menulis Anda dan juga format untuk blog Anda.
    Apakah ini subjek berbayar atau apakah Anda memodifikasinya sendiri?
    Pokoknya teruskan penulisan berkualitas tinggi yang sangat baik,
    jarang terlihat blog yang bagus seperti ini akhir-akhir ini ..


    Permainan poker menggunakan kartu remi yang berjumlah 52 buah kartu, dalam permainan ini setiap pemain harus tahu susunan-susunan kartu poker dari yang paling tinggi sampai dengan yang paling rendah. Tujuan dalam permainan ini adalah pemain mendapatkan susunan kartu yang paling tinggi maka dialah pemenangnya.Anda bisa menguji keberuntungan anda disini idn poker

    ReplyDelete
  47. situs agen judi slot terbaik dan terpercaya,jangan sampai ketinggalan bonus menariknya. daftar dan mainkan sekarang juga, Uji keberuntungan anda disini bersama kami Agen Game Online Resmi dan terpercaya.bandar slot sbobet deposit pulsa

    ReplyDelete