MotionPlus with NunChuck for Arduino 1.0

// .......................................................................
// Code for reading data from a Wii Motion plus device connected to a Nunchuck
// Links to other sites
// http://www.windmeadow.com/node/42   .... first time i saw arduino nunchuk interface
// http://www.kako.com/neta/2009-017/2009-017.html# .....Japanese site with lots of Wii info
// http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus    .... the one and only
// http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html
// ....original motion plus but not passthrough
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/35   .... great Kalman filter code
// thanks to duckhead and knuckles904. I will be using that code for sure.
// http://obex.parallax.com/objects/471/   .... ideas for passthrough
// by Krulkip
// Here is the bitmapping which is changed from the standard nunchuk mapping
// In Nunchuk mode:
//      Bit
// Byte     7    6   5   4   3   2   1   0
// 0     SX<7-----------------------------------------------------0>
// 1     SY<7-----------------------------------------------------0>
// 2     AX<9-----------------------------------------------------2>
// 3     AY<9-----------------------------------------------------2>
// 4     AZ<9---------------------------------------------3>    1
// 5     AZ<2-----1>     AY<1>   AX<1>   BC BZ   0   0
// Please note also the loss of the bit0 resolution.
// Byte 5 Bit 0 and 1 is used for nunchuk (0 0) = nunchuk or motion plus  (1 0) is motion plus detection.
// Byte 4 Bit 0 which is the extention controller detection bit. (1 = extension present)
// Hardware Arduino with ATMega 168
// Connections SCA to AD4 (Analog4) and SCL to AD5 (Analog5)
/*
     Constants for scaling accelerometer values: Example of the accelerometer rotation values for y axis on Ardvark's nunchuk.

     Exact values differ between x and y axis, and probably between nunchuks, but the range value may be constant:

                  298 ayMIN
                  _|_
                /     \
   ayMID 513 - |       | - 513 ayMID
                \ _ _ /
                   |
                  728 ayMAX

     ayRange = (ayMID - ayMIN)*2 = ayMAX - ayMIN = 430
*/
//=========================================================
// changelog
// ---------
// 8/9/2009 - by dimkasta @ www.kastaniotis.net
// Adapted for use with the transistor-less switching code originally written by krulkip on
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/60
// Calibration and utility code based on the code by duckhead
// 9/9/2009 - by dimkasta @ www.kastaniotis.net
// Added Accelerometer calibration
// Removed the casting to double and the multiplying with millis/sec from the cycle saving to further greately
// increase the free time to ~1/50 in comparison to the reading time.
// 18/9/2009 - by Ardvark
// Slow/fast bit reading fixed: Slow degree/sec scale value appears correct.
// Included details for absolute scaling of accelerometer values.
// Included complementary filter for combining single accelerometer/gyro axes to produce drift- and translation-suppressed orientation values.
//=========================================================
// TODO LIST - Known issues
// ---------
// Assign common names to gyro and accel axes
//........................................................................

#include <Wire.h>

unsigned long Now = millis();
unsigned long lastread = Now;
unsigned long lastreadMP = Now;
//The system clock function millis() returns a value in milliseconds.  We need it in seconds.
//Instead of dividing by 1000 we multiply by 0.001 for performance reasons.

boolean debug = true;

boolean calibrating = false;

uint8_t data[6];        // array to store arduino output
int cnt = 0;
int ledPin = 13;
int xID;

//wiibrew info seems to be incorrect with regards to scaling, see instead Xevel's comments: http://www.assembla.com/wiki/show/alicewiimotionplus/slow_and_fast_modes
static const int wmpSlowToDegreePerSec = 16; //when gyro speed 1, 16 units = 1°/s
static const int wmpFastToDegreePerSec = 4; //when gyro speed 0, 4 units = 0,25°/s

// Hardware identifiers. Polling data[5]&0x03 for present device
static const int NC = 0x00;
static const int MP = 0x02;
int currentDevice;

// WM+ state variables - if true, then in slow (high res) mode
boolean slowYaw = 0;
boolean slowPitch = 0;
boolean slowRoll = 0;

int rollScale;
int pitchScale;
int yawScale;

// RK integration not currently in use
struct RungeKutta {
  double val_i_3;
  double val_i_2;
  double val_i_1;
  double previous;
};

struct RungeKutta rollRK;
double roll_intRK;

// Calibrated gyro values (- yaw0 etc)
int yaw = 0;
int pitch = 0;
int roll = 0;
float rollf = 0;

float roll_int = 0;
float roll_int_rad = 0;

float dt; // msec
float dtMP; // this is the time elapsed between readings of the motion plus (MP)

float rollCF = 0;
float pitchCF = 0;

static const double DegreeToRadian = 0.0174532925;  // PI/180

// Complimentary filter: inspired by Shane Colton's description in filter.pdf at http://web.mit.edu/first/segway/segspecs.zip

// Tweak tau to alter the performance of the complimentary filter.
// Increase tau: influence of the gyro input increases (less translation artifacts).
// Reduce tau: influence of the accelerometer input increases (faster drift-correction).
// This filtering is temporal: translations that occur over a time scale shorter than tau will be suppressed,
// and gyro changes that occur over a time scale longer than tau will have their drift corrected.
// NB: changes in the read rate of the gyro (dtMP) will change the filter's performance (change tau to compensate).

static const float tau = 0.95; // unit: seconds.
float compFilter(float prevValue, int gyro, double accel, float timeStep) {
  float a = tau/(tau + dtMP);
  float b = 1 - a;
  return (float) (a*(prevValue + ((gyro*DegreeToRadian)*timeStep)) + (b*accel));
}

// accelerometer values
int ax_m = 0;
int ay_m = 0;
int az_m = 0;

// Exact values differ between x and y axis, and probably between nunchuks, but the range value may be constant:
static const int axMIN = 290;
static const int axMAX = 720;
static const int axMID = (axMAX - axMIN)/2 + axMIN;
static const int axRange = (axMID - axMIN)*2;

static const int ayMIN = 298;
static const int ayMAX = 728;
static const int ayMID = (ayMAX - ayMIN)/2 + ayMIN;
static const int ayRange = (ayMID - ayMIN)*2;

// Not sure what a meaningful scale is for the z accelerometer axis so:

static const int azMID = ayMID;
static const int azRange = ayRange;

// raw 3 axis gyro readings MP+ //gyro readings => yaw/yawscale
int readingsX;
int readingsY;
int readingsZ;

// Nunchuck variables //NunChuck X angle NOT readings
float accelAngleX=0;    //NunChuck X angle
float accelAngleY=0;    //NunChuck Y angle
float accelAngleZ=0;    //NunChuck Z angle
float theta=0;

int z_button = 0;
int c_button = 0;
int joy_x_axis = 0;
int joy_y_axis = 0;

// calibration zeroes
//gyros
int yaw0 = 0;
int pitch0 = 0;
int roll0 = 0;

//accelerometers
int yawAcc0 = 0;
int pitchAcc0 = 0;
int rollAcc0 = 0;

double x;
double y;
double z;


//Nunchuk accelerometer value to radian conversion.
float angleInRadians(int range, int measured) {
  float x = range;
  return (float)(measured/x) * PI;
}

void
setup ()
{
  Serial.begin (115200);
  Wire.begin();

  initializeSensors();
  calibrateZeroes();

  rollRK.val_i_1 = 0;
  rollRK.val_i_2 = 0;
  rollRK.val_i_3 = 0;
  rollRK.previous = 0;

}

void send_zero ()
{
    Wire.beginTransmission(0x52);
    Wire.write((uint8_t)0x00);
    Wire.endTransmission();
}

void loop ()
{
  Now = millis();
  dt = Now - lastread; //compute the time delta since last iteration, in msec.
  if (dt >= 10) //Only process delta angles if at least 1/100 of a second has elapsed. NB: 9 ms is the lowest possible interval before things jam up on an ATmega 328
  {
     readData();
     lastread = Now;
  }
  else // just for demo of the free cycles
  {
  //delay(10);
  //Serial.println("Free cycle");
  }
  delay(100);
}

void readData()
{
  long startReading = millis();
  //digitalWrite(13,HIGH);  // first flash the LED to show activity

    send_zero (); // send the request for next bytes
    //delay (10);

    // now follows conversion command instructing extension controller to get
    // all sensor data and put them into 6 byte register within the extension controller
    Wire.requestFrom (0x52,6);
    data[0] = Wire.read();
    data[1] = Wire.read();
    data[2] = Wire.read();
    data[3] = Wire.read();
    data[4] = Wire.read();
    data[5] = Wire.read();


    parseData();
  if(false)
  {
    long endReading = millis() - startReading;
    Serial.print("Finished Reading in ");
    Serial.print(endReading);
    Serial.println(" milliseconds");
  }
  // digitalWrite(13,LOW);  // first flash the LED to show activity

}

void parseData ()
{
  // check if nunchuk is really connected
  if ((data[4]&0x01)==0x01) {
  //printLine("Ext con: ");

  currentDevice = data[5]&0x03;
  }
    if (currentDevice == NC)
    {

  // Dimitris version of accl readings
      ax_m = (data[2] << 2) + ((data[5] >> 3) & 2) ;
      ay_m = (data[3] << 2) + ((data[5] >> 4) & 2);
      az_m = (data[4] << 2) + ((data[5] >> 5) & 6) ;

        //Zero the values on a#MID.
        ax_m -= axMID;
        ay_m -= ayMID;
        az_m -= azMID;



 // stitch

 // Convert to radians by mapping 180 deg of rotation to PI
        x = angleInRadians(axRange, ax_m);
        y = angleInRadians(ayRange, ay_m);
        z = angleInRadians(azRange, az_m);

        //compute values that are used in multiple places
//        double xSquared = x*x;
//        double ySquared = y*y;
//        double zSquared = z*z;
//
//        accelAngleX = atan(x / sqrt(ySquared + zSquared));
//        accelAngleY = atan(y / sqrt(xSquared + zSquared));
//        theta       = atan(sqrt(xSquared + ySquared) / z);

   }
  else
  if  (currentDevice == MP)
  {
    dtMP = (Now - lastreadMP)/1000.0; //compute the time delta since last MP read, in sec.
    lastreadMP = Now;

      // duck's hardware setup for this code has the MP spun 90 deg relative to the NC, so I swapped the roll and pitch to match a setup with the NC and MP on the same axis
      // Gyroscopes: dual-axis IDG-600, see http://www.invensense.com/products/idg_600.html and EPSON TOYOCOM labelled X3500W (yaw)
    roll = (((data[4]&0xFC)<<6) + data[1]);
    pitch = (((data[5]&0xFC)<<6) + data[0]);
    yaw = (((data[3]&0xFC)<<6) + data[2]);

    if(calibrating == false) //Dont compensate if we are doing the calibration measurements.
    {

      slowPitch = (data[4]&0x02)>>1;
      slowRoll = (data[3]&0x01)>>0;
      slowYaw = (data[3]&0x02)>>1;

      rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec; // if speed == 1, then wmpSlowToDegreePerSec, else wmpFastToDegreePerSec
      pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;
      yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;

      //Calibrating
      roll -= roll0;
      pitch -= pitch0;
      yaw -= yaw0;

      roll = roll/rollScale;    // deg/s = mV/(mV/deg/s)
      pitch = pitch/pitchScale;
      yaw = yaw/yawScale;

      // Complimentary filter: inspired by Shane Colton's description in filter.pdf at http://web.mit.edu/first/segway/segspecs.zip
        rollCF = compFilter(rollCF, roll, x, dtMP);
        pitchCF = compFilter(pitchCF, pitch, y, dtMP);

    }

  }


  if(debug)
  {
    //Uncomment the set that you want to monitor.
    //Having everything spammed is not very helpful

   // Serial.print ("joyx= ");
   // Serial.println (joy_x_axis);

   // Serial.print ("joyy= ");
   // Serial.println (joy_y_axis);
//
//     Serial.print (" ");
//     Serial.print(slowRoll==1);
//
//    Serial.print (" ");
//    Serial.print(slowPitch==1);
//
//    Serial.print (" ");
//    Serial.print(slowYaw==1);
//
//     Serial.print (" ");
//     Serial.print (rollScale);
//
//    Serial.print (" ");
//    Serial.print (x);

//    Serial.print (" ");
//    Serial.print (y);

//    Serial.print (" ");
//    Serial.print (z);

//    Serial.print (" ");
//    Serial.println (rollCF);

//    Serial.print (" ");
//    Serial.println (pitchCF);
//
//    Serial.print (" ");
//    Serial.println (a);
//
//    Serial.print (" ");
//    Serial.print (accelAngleX);
//
//    Serial.print (" ");
//    Serial.print (accelAngleY);
//
//    Serial.print (" ");
//    Serial.println (theta);
//
//    Serial.print (" ");
//    Serial.print (ax_m);
//
//    Serial.print (" ");
//    Serial.print (ay_m);
////
//    Serial.print (" ");
//    Serial.print (az_m);

//    Serial.print (" ");
//    Serial.print (ax_m_rad);
//
//    Serial.print (" ");
//    Serial.print (ay_m_rad);
//
//    Serial.print (" ");
//    Serial.println (az_m_rad);

   // if (z_button == 0) { Serial.println ("z_button "); }
   // if (c_button == 0) { Serial.println("c_button "); }
//
     Serial.print (" ");
     Serial.print (roll);
//
     Serial.print (" ");
     Serial.print (pitch);
//
     Serial.print (" ");
     Serial.println (yaw);
//
//     Serial.print (" ");
//     Serial.print(slowPitch);
//     Serial.print (" ");
//     Serial.println(slowYaw);
//     Serial.print (" ");
//     Serial.println(dt);
  }
  // Finished with data assignment, now we need calibration

}

void initializeSensors()
{

  digitalWrite(13,HIGH);  // first flash the LED to show activity
  Serial.println ("Now detecting WM+");
  //delay(100);
  // now make Wii Motion plus the active extension
  // nunchuk mode = 05, wm+ only = 04, classic controller = 07
  Serial.println ("Activating WM+ in NC mode (5)...");
   Wire.beginTransmission(0x53);
   Wire.write((uint8_t)0xFE);
   Wire.write((uint8_t)0x05);// nunchuk
   Wire.endTransmission();
  Serial.println (" OK done");
   //delay (100);
  // now innitiate Wii Motion plus
   Serial.println ("Innitialising Wii Motion plus ........");
    Wire.beginTransmission(0x53);
    Wire.write((uint8_t)0xF0);
    Wire.write((uint8_t)0x55);
    Wire.endTransmission();
  Serial.println(" OK done");
    //delay (100);
  Serial.println ("Reading address at 0xFA .......");
    Wire.beginTransmission(0x52);
    Wire.write((uint8_t)0xFA);
    Wire.endTransmission();
  Serial.println(" OK done");
   //delay (100);
   Wire.requestFrom (0x52,6);
   data[0] = Wire.read();//Serial.print(outbuf[0],HEX);Serial.print(" ");
   data[1] = Wire.read();//Serial.print(outbuf[1],HEX);Serial.print(" ");
   data[2] = Wire.read();//Serial.print(outbuf[2],HEX);Serial.print(" ");
   data[3] = Wire.read();//Serial.print(outbuf[3],HEX);Serial.print(" ");
   data[4] = Wire.read();//Serial.print(outbuf[4],HEX);Serial.print(" ");
   data[5] = Wire.read();//Serial.print(outbuf[5],HEX);Serial.print(" ");

   xID= data[0] + data[1] + data[2] + data[3] + data[4] + data[5];
  Serial.println("Extension controller xID = 0x");
   Serial.print(xID,HEX);
   if (xID == 0xCB) { Serial.println ("MP+ on but not active"); }
   if (xID == 0xCE) { Serial.println ("MP+ on and active"); }
   if (xID == 0x00) { Serial.println ("MP+ not on"); }
   //delay (100);
   // Now we want to point the read adress to 0xa40008 where the 6 byte data is stored
  Serial.println ("Reading address at 0x08 .........");
    Wire.beginTransmission(0x52);
    Wire.write((uint8_t)0x08);
    Wire.endTransmission();
  Serial.println(" OK done");
  Serial.println ("Finished setup");

  digitalWrite(13,LOW); // Led off
}

/*
 * Find the steady-state of the WM+ gyros.  This function reads the gyro values 100 times and averages them.
 * Those values are used as the baseline steady-state for all subsequent reads.  As a result it is very
 * important that the device remain relatively still during startup.
 */


void calibrateZeroes(){  // wiibrew: While the Wiimote is still, the values will be about 0x1F7F (8,063)
  calibrating = true;
  if(debug)
  {
    Serial.println("Starting calibration...");
  }
  //Gyros
  long y0 = 0;
  long p0 = 0;
  long r0 = 0;

  //Accelerometers for application calibration, not for zeroing the starting points.
  long ya0 = 0;
  long pa0 = 0;
  long ra0 = 0;

  const int avg = 200; //100 reads of the gyros and 100 of the accelerometers

  digitalWrite(13,HIGH);  // first flash the LED to show activity

  for (int i=0;i<avg; i++)
  {
    delay(10); // bypasses the normal global delays
    readData();

    if(currentDevice == MP)
    {
      if(debug)
      {
        Serial.println("<= Data from MP");
      }

      y0+=yaw;
      r0+=roll;
      p0+=pitch;
    }
    else
    {
       if(debug)
      {
        Serial.println("<= Data from NC...");
      }
      ya0 += az_m;
      pa0 += ay_m;
      ra0 += ax_m;

    }
  }

  yaw0 = y0/(avg/2);
  roll0 = r0/(avg/2);
  pitch0 = p0/(avg/2);

  yawAcc0 = ya0/(avg/2);
  rollAcc0 = ra0/(avg/2);
  pitchAcc0 = pa0/(avg/2);

  calibrating = false;

  digitalWrite(13,LOW); // Led off
  if(debug)
  {
    Serial.println("Calibration results...");
    Serial.print("Yaw0:");
    Serial.println(yaw0);
    Serial.print("Roll0:");
    Serial.println(roll0);
    Serial.print("Pitch0:");
    Serial.println(pitch0);
    Serial.print("YawAcc0:");
    Serial.println(yawAcc0);
    Serial.print("RollAcc0:");
    Serial.println(rollAcc0);
    Serial.print("PitchAcc0:");
    Serial.println(pitchAcc0);
  }
}

/*
 * Compute the common fourth-order Runge-Kutta algorithm as part of the integrating the gyro's yaw signal.  This
 * will smooth the values a tad.  Gyro drift is still a problem, however.
 *
 * rk       the RungaKutta struct to hold previous values
 * val_i_0  the latest raw value to integrate
 *
 * returns the RK4 approximation of all values up until time t
 */
double computeRK4(struct RungeKutta *rk, double val_i_0) {
  rk->previous += 0.16667*(rk->val_i_3 + 2*rk->val_i_2 + 2*rk->val_i_1 + val_i_0);
  rk->val_i_3 = rk->val_i_2;
  rk->val_i_2 = rk->val_i_1;
  rk->val_i_1 = val_i_0;
  return rk->previous;
}