Wii Motion Plus Decoded

Ok, Im finished decoding the Wii motion plus peripheral. Ive got it up and running on my arduino nano. All the info as well as the demo code is on my shiny new blogger account. Check it out: http://randomhacksofboredom.blogspot.com/

Ill also post code here as its more tidy and suited for that sort of thing

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

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);
}

Words cannot express how awesome this is.

When I first heard of the Motion Plus I knew that Arduino IMU would one day arrive. I didn't know that it would be so soon.

Congratulations knuckles904.

which resolution in parts of degree do you get with the WII Motion Plus?

kudos :) can't wait to get down and dirty with this

which resolution in parts of degree do you get with the WII Motion Plus?

2nd that! and how fast is it?

but its shure the cheapest sensor you can get :D

The MotionPlus uses the IDG600 dual axis gyro which is specified at 2000 degrees per second with a linearity of 0.1% http://www.invensense.com/products/idg_600.html

Great stuff!

I'd just assumed that Motion Plus added sensors to deal with the lack of yaw data on the Wii-remote and you needed to combine Motion Plus with the remote to get complete motion tracking. It seems this isn't the case :D

I'd just assumed that Motion Plus added sensors to deal with the lack of yaw data on the Wii-remote and you needed to combine Motion Plus with the remote to get complete motion tracking. It seems this isn't the case

why? I think the Motion Plus just adds gyros, the accelerometers from the remote are still needed

Ah - my misunderstanding: re-reading your blog more carefully (i.e. being less excited and paying proper attention :-[ ) I see you mention that you need to combine it with the nunchuck/remote to get full motion sensing. Having said that the arrows on the diagram at the bottom do give the impression that the Motion Plus returns roll, pitch and yaw data. It might be worth clarifying that so over-excited fools such as myself don't misinterpret it ;)

:) Thanks knuckles, you rock! Have you figured out how to talk to a nunchuck plugged into the m+ after the m+ has been activated?

Thanks everyone. Sorry about the misunderstanding. The WM+ only contains gyros (specifically a two axis gyro listed above and then another gyro whose datasheet has yet to be found, plus a 14 bit adc). Gyros only yield data about rotational velocity, not position. Accelerometers (like those found in the nunchuck)yield position data (due to gravitational acceleration) and direct acceleration due to motion. So with these two sensors in the three directions, we have a 6DOF IMU (inertial measurement unit) which fully measures motion in three directions.

And sorry xandar, i havent been able to play with that yet. My nunchuck is about 60 miles away right now. The wiibrew site i referenced makes it seem like you have to deactivate the WM+ and then initialize the nunchuck to read from there, but that just doesnt seem right. Ill ask anyone who has both a WM+, nunchuck, and arduino on hand to try to run the standard nunchuck code and see if it works and post back here. If not, I might have something that could do the trick.

(edit)-nevermind, nunchuck and WM+ are at the same address when the WM+ is active. It doesnt seem like anyone has figured out how to read from the expansion slot yet.

Ok, that's what I thought, thanks.

Seems like there must be some way.... disabling (and reinitializing) the WM+ every time you want to read the nunchuck seems like it would hurt your sampling rate too much :-/

Very cool, awesome work knuckles!

The Wiimote and the Nunchuck use the same 3d accelerator part. They get a rate of change of velocity in x, y, z.

Since gravity is a known component, you can use this to estimate current roll and current pitch but not current yaw. The estimates are not very accurate if there are other strong acceleration components like shaking.

The Wiimote has a camera that can analyze the picture to find the “sensor bar” lights. It can then estimate current yaw, and fine-tune the current roll and pitch estimates. The Nunchuck does not have any way of detecting the sensor bar, of course.

With acceleration and a known starting condition, you can also estimate velocity, and with velocity, you can also estimate absolute position. These sensors are pretty noisy, so don’t expect much in the way of position tracking.

The Wii Motion Plus adds a three axis gyro sensor to the above capabilities. It’s not able to determine current yaw, pitch or roll. It’s only able to determine rate of change in yaw, pitch and roll.

Just as with orthogonal velocity, you can also estimate the absolute rotational position (current yaw, current pitch, current roll) given the rotational velocity and a known starting condition.

can someone with a wii, nunchuck, and motion plus please check this for me, i havent been able to get a good answer with google and i dont own a wii(though now i have an 80$ controller for it). With the motion plus plugged into the wiimote, and the nunchuck plugged into the motion plus, will the wii, recognize the nunchuck in a game that needs the nunchuck? I have heard rumors that it will not recognize the nunchuck when plugged into the expansion port. Can anyone confirm or deny this rumor?

(edit) nevermind, this person took apart their WM+ and caused a loose wire

Sir, I should be very indebted to you, if you could help me with some indications about the physical correspondence, in degree par sencond, and the retourned values of the Wii Motion Plus. Yours sincerely, Georges

The wiibrew site I linked suggests that the output of the sensor should be divided by 20 to produce the output in degree/second but Im not seeing how they got that number. The spec sheet for the two axis gyro is known: http://www.invensense.com/products/idg_600.html but there is no definite spec sheet for the last axis. Only one with similar numbering: http://www.epsontoyocom.co.jp/english/product/Sensor/set01/xv3500cb/index.html. Also, it has been speculated that sensitivity of the gyros is programmable, but that hasnt been decoded yet.

So for now, experimental analysis is the best way. Until someone can test it out on a turntable (hint, hint) use raw data/20=deg/sec

For anyone wanting a graphical view of this gyro data, Ive got a little present for you. I modified the processing and arduino code from this post: http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1246274143 to work with the WM+. Now you can actually “see” the data instead of random numbers. Also very useful when you want to test out filters. Many thanks to glacialwanderer for this great code

Processing Code:

// Maurice Ribble (modified by knuckles904 to be used in Wii Motion Plus)
// 6-30-2009
// Original code at http://www.glacialwanderer.com/hobbyrobotics

// This takes data off the serial port and graphs it.
// There is an option to log this data to a file.

// I wrote an arduino app that sends data in the format expected by this app.
// The arduino app sends gyroscope data.

import processing.serial.*;

// Globals
int g_winW             = 820;   // Window Width
int g_winH             = 600;   // Window Height
boolean g_dumpToFile   = false;  // Dumps data to c:\\output.txt in a comma seperated format (easy to import into Excel)
boolean g_enableFilter = false;  // Enables simple filter to help smooth out data.

cDataArray pitch0    = new cDataArray(200);
cDataArray yaw0    = new cDataArray(200);
cDataArray roll0    = new cDataArray(200);
cDataArray pitch      = new cDataArray(200);
cDataArray yaw     = new cDataArray(200);
cDataArray roll     = new cDataArray(200);
cGraph g_graph         = new cGraph(10, 190, 800, 400);
Serial g_serial;
PFont  g_font;

void setup()
{
  size(g_winW, g_winH, P2D);

  println(Serial.list());
  g_serial = new Serial(this, "COM5", 115200, 'N', 8, 1.0);  //enter COM port of 
  g_font = loadFont("ArialMT-20.vlw");                       //of arduino here
  textFont(g_font, 20);
  
  // This draws the graph key info
  strokeWeight(1.5);
  stroke(255, 0, 0);     line(20, 440, 35, 440);
  stroke(0, 255, 0);     line(20, 460, 35, 460);
  stroke(0, 0, 255);     line(20, 480, 35, 480);
  stroke(255, 0, 0);     line(20, 500, 35, 500);
  stroke(0, 255, 0);     line(20, 520, 35, 520);
  stroke(0, 0, 255);     line(20, 540, 35, 540);
  fill(0, 0, 0);
  text("pitch0", 40, 450);
  text("yaw0", 40, 470);
  text("roll0", 40, 490);
  text("pitch", 40, 510);
  text("yaw", 40, 530);
  text("roll", 40, 550);
  //text("current raw", 180, 430);
  //text("current deg/s", 320, 430);
  
  if (g_dumpToFile)
  {
    // This clears deletes the old file each time the app restarts
    byte[] tmpChars = {'\r', '\n'};
    saveBytes("c:\\output.txt", tmpChars);
  }
}

void draw()
{
  // We need to read in all the avilable data so graphing doesn't lag behind
  while (g_serial.available() >= 2*6+2)
  {
    processSerialData();
  }

  strokeWeight(1);
  fill(255, 255, 255);
  g_graph.drawGraphBox();
  
  strokeWeight(1.5);
  stroke(255, 0, 0);
  g_graph.drawLine(pitch0, 0, 16384);
  stroke(0, 255, 0);
  g_graph.drawLine(yaw0, 0, 16384);
  stroke(0, 0, 255);
  g_graph.drawLine(roll0, 0, 16384);
  stroke(255, 0, 0);
  g_graph.drawLine(pitch, 0, 16384);
  stroke(0, 255, 0);
  g_graph.drawLine(yaw, 0, 16384);
  stroke(0, 0, 255);
  g_graph.drawLine(roll, 0, 16384);
}

// This reads in one set of the data from the serial port
void processSerialData()
{
  int inByte = 0;
  int curMatchPos = 0;
  int[] intBuf = new int[2];

  intBuf[0] = 0xAD;
  intBuf[1] = 0xDE;
  
  while (g_serial.available() < 2); // Loop until we have enough bytes
  inByte = g_serial.read();
  
  // This while look looks for two bytes sent by the client 0xDEAD
  // This allows us to resync the server and client if they ever
  // loose sync.  In my testing I haven't seen them loose sync so
  // this could be removed if you need to, but it is a good way to
  // prevent catastrophic failure.
  while(curMatchPos < 2)
  {
    if (inByte == intBuf[curMatchPos])
    {
      ++curMatchPos;
      
      if (curMatchPos == 2)
        break;
    
      while (g_serial.available() < 2); // Loop until we have enough bytes
      inByte = g_serial.read();
    }
    else
    {
      if (curMatchPos == 0)
      {
        while (g_serial.available() < 2); // Loop until we have enough bytes
        inByte = g_serial.read();
      }
      else
      {
        curMatchPos = 0;
      }
    }
  }
  
  while (g_serial.available() < 2*6);  // Loop until we have a full set of data

  // This reads in one set of data
  {
    byte[] inBuf = new byte[2];
    int pitch0_cur, yaw0_cur, roll0_cur, pitch_cur, yaw_cur, roll_cur;
  
    g_serial.readBytes(inBuf);
    // Had to do some type conversion since Java doesn't support unsigned bytes
    pitch0_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    g_serial.readBytes(inBuf);
    yaw0_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    g_serial.readBytes(inBuf);
    roll0_cur = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    g_serial.readBytes(inBuf);
    pitch_cur   = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    g_serial.readBytes(inBuf);
    yaw_cur  = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    g_serial.readBytes(inBuf);
    roll_cur  = ((int)(inBuf[1]&0xFF) << 8) + ((int)(inBuf[0]&0xFF) << 0);
    
    pitch0.addVal(pitch0_cur);
    yaw0.addVal(yaw0_cur);
    roll0.addVal(roll0_cur);
    pitch.addVal(pitch_cur);
    yaw.addVal(yaw_cur);
    roll.addVal(roll_cur);

    if (g_dumpToFile)  // Dump data to a file if needed
    {
      String tempStr;
      tempStr = pitch0_cur + "," + yaw0_cur + "," + roll0_cur + "," + pitch_cur + "," + yaw_cur + "," + roll_cur + "\r\n";
      FileWriter file;

      try  
      {  
        file = new FileWriter("c:\\output.txt", true); //bool tells to append
        file.write(tempStr, 0, tempStr.length()); //(string, start char, end char)
        file.close();
      }  
      catch(Exception e)  
      {  
        println("Error: Can't open file!");
      }
    }

    /*
    print(pitch0_cur);  print(" ");   print(yaw0_cur);   print(" ");    print(roll0_cur);     print(" ");
    print(pitch_cur);    print(" ");   print(yaw_cur);    print(" ");    println(roll_cur);
    */
  }
}

// This class helps mangage the arrays of data I need to keep around for graphing.
class cDataArray
{
  float[] m_data;
  int m_maxSize;
  int m_startIndex = 0;
  int m_endIndex = 0;
  int m_curSize;
  
  cDataArray(int maxSize)
  {
    m_maxSize = maxSize;
    m_data = new float[maxSize];
  }
  
  void addVal(float val)
  {
    
    if (g_enableFilter && (m_curSize != 0))
    {
      int indx;
      
      if (m_endIndex == 0)
        indx = m_maxSize-1;
      else
        indx = m_endIndex - 1;
      
      m_data[m_endIndex] = getVal(indx)*.5 + val*.5;
    }
    else
    {
      m_data[m_endIndex] = val;
    }
    
    m_endIndex = (m_endIndex+1)%m_maxSize;
    if (m_curSize == m_maxSize)
    {
      m_startIndex = (m_startIndex+1)%m_maxSize;
    }
    else
    {
      m_curSize++;
    }
  }
  
  float getVal(int index)
  {
    return m_data[(m_startIndex+index)%m_maxSize];
  }
  
  int getCurSize()
  {
    return m_curSize;
  }
  
  int getMaxSize()
  {
    return m_maxSize;
  }
}

// This class takes the data and helps graph it
class cGraph
{
  float m_gWidth, m_gHeight;
  float m_gLeft, m_gBottom, m_gRight, m_gTop;
  
  cGraph(float x, float y, float w, float h)
  {
    m_gWidth     = w;
    m_gHeight    = h;
    m_gLeft      = x;
    m_gBottom    = g_winH - y;
    m_gRight     = x + w;
    m_gTop       = g_winH - y - h;
  }
  
  void drawGraphBox()
  {
    stroke(0, 0, 0);
    rectMode(CORNERS);
    rect(m_gLeft, m_gBottom, m_gRight, m_gTop);
  }
  
  void drawLine(cDataArray data, float minRange, float maxRange)
  {
    float graphMultX = m_gWidth/data.getMaxSize();
    float graphMultY = m_gHeight/(maxRange-minRange);
    
    for(int i=0; i<data.getCurSize()-1; ++i)
    {
      float x0 = i*graphMultX+m_gLeft;
      float y0 = m_gBottom-((data.getVal(i)-minRange)*graphMultY);
      float x1 = (i+1)*graphMultX+m_gLeft;
      float y1 = m_gBottom-((data.getVal(i+1)-minRange)*graphMultY);
      line(x0, y0, x1, y1);
    }
  }
}

And the new Arduino Code to go with it:

#include <Wire.h>

byte data[6];          //six data bytes
int yaw, pitch, roll;  //three axes
int yaw0, pitch0, roll0;  //calibration zeroes
int startTag=0xDEAD;

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

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.write((unsigned byte*)&startTag, 2);  //see diagram on randomhacksofboredom.blogger.com
  Serial.write((unsigned byte*)&pitch0, 2);    //for info on which axis is which
  Serial.write((unsigned byte*)&yaw0, 2);
  Serial.write((unsigned byte*)&roll0, 2);
  Serial.write((unsigned byte*)&pitch, 2);
  Serial.write((unsigned byte*)&yaw, 2);
  Serial.write((unsigned byte*)&roll, 2);
  delay(10);
}

Oh yea, you have to have 0016 to use this code, or it wont compile

Thank you very much for this code knuckles904 - especially the 'gift' here at the end.

Im presently working on trying to integrate this with a Nunchuck. From what I can determine, with the Nunchuck plugged into the rear of the MP, I need to re-init the Nunchuck, take some readings, re-activate the MP, take some readings, then loop ?