// Modified Kalman code using Roll, Pitch, and Yaw from a Wii MotionPlus and X, Y, and Z accelerometers from a Nunchuck.// Also uses "new" style Init to provide unencrypted data from the Nunchuck to avoid the XOR on each byte.// Requires the WM+ and Nunchuck to be connected to Arduino pins D3/D4 using the schematic from Knuckles904 and johnnyonthespot// See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html// Kalman Code By Tom Pycke. http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data// Original zipped source with very instructive comments: http://tom.pycke.be/file_download/4//// Some of this code came via contributions or influences by Adrian Carter, Knuckles904, evilBunny, Ed Simmons, & Jordi Munoz//// There are two coordinate systems at play here:// The body-fixed coordinate system, which is fixed to the platform. That is, the x-axis is always pointing to the nose of the// aircraft (nosecone on a rocket), the y-axis points to the right, and the z-axis points down on an airplane, (parallel to the// earth away from you on a rocket).//// The aircraft will move with respect to the navigation frame, which is a fixed coordinate system.  // In that frame, the x-axis points north, the y-axis points east and the z-axis points down. This is called the "NED system", or// the "Local Tangent Plane" or the "Local Geodetic Frame".

// Created by Duckhead   v0.6   Yaw is done via integration of gyro data and smoothed via a Runge-Kutta 4, but there are still issues with it

#include <math.h>#include <Wire.h>/////////////////////////////////////////

struct GyroKalman{  /* These variables represent our state matrix x */  double x_angle,         x_bias;

 /* Our error covariance matrix */  double P_00,         P_01,         P_10,         P_11;

 /*   * Q is a 2x2 matrix of the covariance. Because we   * assume the gyro and accelerometer noise to be independent   * of each other, the covariances on the / diagonal are 0.   *   * Covariance Q, the process noise, from the assumption   *    x = F x + B u + w   * with w having a normal distribution with covariance Q.   * (covariance = E[ (X - E[X])*(X - E[X])' ]   * We assume is linear with dt   */  double Q_angle, Q_gyro;

 /*   * Covariance R, our observation noise (from the accelerometer)

  * Also assumed to be linear with dt   */  double R_angle;};

struct RungeKutta {  double val_i_3;  double val_i_2;  double val_i_1;  double previous;};

//Precomputed constants to avoid floating point division at runtime (too many clock cycles)static const double RadianToDegree = 57.2957795;    // 180/PIstatic const double DegreeToRadian = 0.0174532925;  // PI/180

//The WMP has two modes: 2000 deg/s (slow) and 500 deg/s (fast).  It is assumed that the sensitivity of the output follows//the same ratio (it seems that the sensitivity is 20 mV/deg/s and 0.5 mV/deg/s), which is to say it's proportional to 4:1.static const int wmpSlowToDegreePerSec = 20;static const int wmpFastToDegreePerSec = wmpSlowToDegreePerSec/5;

//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.static const double SecondsPerMillis = 0.001;

//WM+ variables/* * R represents the measurement covariance noise.  In this case, * it is a 1x1 matrix that says that we expect 0.3 rad jitter * from the accelerometer. */static const float R_angle = .3; //.3 default

/* * Q is a 2x2 matrix that represents the process covariance noise. * In this case, it indicates how much we trust the acceleromter * relative to the gyros. * * You should play with different values here as the effects are interesting. Over prioritizing the * accelerometers results in fairly inaccurate results. */static const double  Q_angle = 0.002; //(Kalman)static const double  Q_gyro  = 0.1; //(Kalman)

// Kalman data structures for each rotational axisstruct GyroKalman rollData;struct GyroKalman pitchData;struct GyroKalman yawData;struct RungeKutta yawRK;

// raw 3 axis gyro readingsint readingsX;int readingsY;int readingsZ;  

byte data[6]; //six data bytes

// normalized (scaled per WM+ mode) rotation valuesint yaw = 0;

int pitch = 0;int roll = 0;

// calibration zeroesint yaw0 = 0;int pitch0 = 0;int roll0 = 0;

// WM+ state variables - if true, then in slow (high res) modebool slowYaw;bool slowPitch;bool slowRoll;bool debug = false;

// Nunchuck variables

float accelAngleX=0; //NunChuck X anglefloat accelAngleY=0; //NunChuck Y anglefloat accelAngleZ=0; //NunChuck Z anglefloat theta=0;          //Angle of the z-axis to vertical (not the same as yaw)

// Internal rogram state variablesunsigned long lastread=0; //last system clock in millis

/* * Nunchuk accelerometer value to radian conversion.   Normalizes the measured value * into the range specified by hi-lo.  Most of the time lo should be 0, and hi should be 1023. * * lo         the lowest possible reading from the Nunchuk * hi         the highest possible reading from the Nunchuk * measured   the actual measurement */float angleInRadians(int lo, int hi, int measured) {  float x = (hi - lo);  return (float)(measured/x) * PI;}

/* * Switch control from WM+ to Nunchuck via I2C. */void switchtonunchuck(){  digitalWrite(3, LOW);  digitalWrite(4, LOW);  digitalWrite(4, HIGH);}

/* * Switch control from Nunchuck to WM+ via I2C. */void switchtowmp(){  digitalWrite(3, LOW);  digitalWrite(4, LOW);  digitalWrite(3, HIGH);}

/* * Initialize Nunchuck. */void nunchuck_init () //(nunchuck){  // Uses New style init - no longer encrypted so no need to XOR bytes later... might save some cycles

 Wire.beginTransmission (0x52);// transmit to device 0x52  Wire.send (0xF0); // sends memory address  Wire.send (0x55); // sends sent a zero.  Wire.endTransmission (); // stop transmitting  delay(100);  Wire.beginTransmission (0x52);// transmit to device 0x52  Wire.send (0xFB); // sends memory address  Wire.send (0x00); // sends sent a zero.  Wire.endTransmission (); // stop transmitting}

/* * Initialize everything. */void setup(){  Serial.begin(115200);

 pinMode(3, OUTPUT);  pinMode(4, OUTPUT);  digitalWrite(3, HIGH);  digitalWrite(4, HIGH);

 Wire.begin (); // join i2c bus with address 0x52 (nunchuck)  switchtowmp();  wmpOn();  calibrateZeroes();  switchtonunchuck();  nunchuck_init (); // send the initilization handshake

 initGyroKalman(&rollData, Q_angle, Q_gyro, R_angle);  initGyroKalman(&pitchData, Q_angle, Q_gyro, R_angle);  initGyroKalman(&yawData, Q_angle, Q_gyro, R_angle);    yawRK.val_i_1 = 0;  yawRK.val_i_2 = 0;  yawRK.val_i_3 = 0;  yawRK.previous = 0;  lastread = millis();}

/* * Initialize the kalman structures. * * kalman    the kalman data structure * Q_angle   the process covariance noise for the accelerometers * Q_gyro    the process covariance noise for the gyros * R_angle   the measurement covariance noise (jitter in the accelerometers) */void initGyroKalman(struct GyroKalman *kalman, const float Q_angle, const float Q_gyro, const float R_angle) {  kalman->Q_angle = Q_angle;  kalman->Q_gyro  = Q_gyro;  kalman->R_angle = R_angle;    kalman->P_00 = 0;  kalman->P_01 = 0;  kalman->P_10 = 0;  kalman->P_11 = 0;}

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.endTransmission();}

void wmpSendZero(){  Wire.beginTransmission(0x52);//now at address 0x52  Wire.send(0x00); //send zero to signal we want info  Wire.endTransmission();}

/* * 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(){  long y0 = 0;  long p0 = 0;  long r0 = 0;  const int avg = 100;  for (int i=0;i<avg; i++){    wmpSendZero();    Wire.requestFrom(0x52,6);    for (int t=0;t<6;t++){      data[t]=Wire.receive();    }    y0+=(((data[3] >> 2) << 8)+data[0]);    r0+=(((data[4] >> 2) << 8)+data[1]);    p0+=(((data[5] >> 2) << 8)+data[2]);  }    yaw0 = y0/avg;  roll0 = r0/avg;  pitch0 = p0/avg;}

//STITCH

//STITCH

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];  roll=((data[4] >> 2) << 8)+data[1];  pitch=((data[5] >> 2) << 8)+data[2];    slowPitch = data[3] & 1;  slowYaw = data[3] & 2;  slowRoll = data[4] & 2;

 //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus  //for info on what each byte represents  yaw -= yaw0;  roll -= roll0;  pitch -= -pitch0;}

void loop(){  unsigned long now = millis();  double dt = ((double)(now - lastread)) * SecondsPerMillis; //compute the time delta since last iteration, in seconds.

 if (dt >= 0.01) {  //Only process delta angles if at least 1/100 of a second has elapsed    lastread = now;

   switchtowmp();    receiveData();    switchtonunchuck();    readNunchuck();

   // Handle the fast/slow bits of the Wii MotionPlus    const int rollScale  = slowRoll  ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;    const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;    const int yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;        readingsX = (roll/rollScale); // read from the gyro sensor    readingsY = (pitch/pitchScale); // read from the gyro sensor    readingsZ = (yaw/yawScale); // read from the gyro sensor        predict(&rollData, readingsX * DegreeToRadian, dt);    predict(&pitchData, readingsY * DegreeToRadian, dt);      double rollAngle = update(&rollData, accelAngleX);    double pitchAngle = update(&pitchData, accelAngleY);    double yawAngle = computeRK4(&yawRK, readingsZ*DegreeToRadian*dt);

//      Serial.print(" Roll: ");      Serial.print(rollAngle*RadianToDegree);  Serial.print(", ");//      Serial.print(" Pitch: ");      Serial.print(pitchAngle*RadianToDegree);  Serial.print(", ");//      Serial.print(" Yaw: ");      Serial.print(yawAngle*RadianToDegree);  Serial.print(", ");  Serial.print(1);  Serial.print(",");  Serial.println(2);//      Serial.print(" Theta: ");//      Serial.println(theta*RadianToDegree);  }}

/* * The kalman predict method.  See http://en.wikipedia.org/wiki/Kalman_filter#Predict

* * kalman    the kalman data structure * dotAngle  Derivitive Of The (D O T) Angle.  This is the change in the angle from the gyro.  This is the value from the *           Wii MotionPlus, scaled to fast/slow. * dt        the change in time, in seconds; in other words the amount of time it took to sweep dotAngle * * Note: Tom Pycke's ars.c code was the direct inspiration for this. However, his implementation of this method was inconsistent *       with the matrix algebra that it came from.  So I went with the matrix algebra and tweaked his implementation here. */void predict(struct GyroKalman *kalman, double dotAngle, double dt) {

 kalman->x_angle += dt * (dotAngle - kalman->x_bias);  kalman->P_00 += -1 * dt * (kalman->P_10 + kalman->P_01) + dt*dt * kalman->P_11 + kalman->Q_angle;  kalman->P_01 += -1 * dt * kalman->P_11;  kalman->P_10 += -1 * dt * kalman->P_11;  kalman->P_11 += kalman->Q_gyro;}

/* * The kalman update method.  See http://en.wikipedia.org/wiki/Kalman_filter#Update * * kalman    the kalman data structure * angle_m   the angle observed from the Wii Nunchuk accelerometer, in radians */double update(struct GyroKalman *kalman, double angle_m) {  const double y = angle_m - kalman->x_angle;  const double S = kalman->P_00 + kalman->R_angle;  const double K_0 = kalman->P_00 / S;  const double K_1 = kalman->P_10 / S;

 kalman->x_angle += K_0 * y;  kalman->x_bias  += K_1 * y;    kalman->P_00 -= K_0 * kalman->P_00;  kalman->P_01 -= K_0 * kalman->P_01;  kalman->P_10 -= K_1 * kalman->P_00;  kalman->P_11 -= K_1 * kalman->P_01;    return kalman->x_angle;}

/* * 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;}

/* * Write a zero byte to the nunchuck. */void send_zero () //(nunchuck){  Wire.beginTransmission (0x52); // transmit to device 0x52 (nunchuck)  Wire.send (0x00);              // sends one byte (nunchuck)  Wire.endTransmission ();       // stop transmitting (nunchuck)}

/* * Request and read the current accelerometer values from the Nunchuck. */void readNunchuck () {    int cnt = 0;    byte outbuf[6];            // array to store NunChuck output

   Wire.requestFrom (0x52, 6); // request data from nunchuck

   while (Wire.available ()) //(NunChuck)    {      outbuf[cnt] = Wire.receive (); // receive byte as an integer //(NunChuck)      cnt++;//(NunChuck)    }

   send_zero (); // send the request for next bytes    // If we received the 6 bytes, then process them //(NunChuck)    if (cnt >= 5) //(NunChuck)    {

processAccelerometers (outbuf); //(NunChuck)    }}

/* * This function processes the byte array from the wii nunchuck. * * The execution of this function results in the updating of 3 globals, accelAngleX, accelAngleY, and accelAngleZ with the angle in radians. * If the Wii Motion Plus and the Nunchuck are oriented together in the way this code assumes then: *    Nunchuck X maps to Wii Motion Plus Roll *    Nunchuck Y maps to Wii Motion Plus Pitch *    Nunchuck Z maps to Wii Motion Plus Yaw * */void processAccelerometers (byte outbuf[]){  //Pull the bits out of the array for each axis.  Other implementations floating around are incorrect with the way  //they operate on the lower 2 bits.  Thanks to evilBunny for this code.  int ax_m = (outbuf[2] << 2) + ((outbuf[5] >> 2) & 0x03);  int ay_m = (outbuf[3] << 2) + ((outbuf[5] >> 4) & 0x03);  int az_m = (outbuf[4] << 2) + ((outbuf[5] >> 6) & 0x03);

 //The nunchuk accelerometers read ~511 steady state.  Remove that to zero the values.  ax_m -= 511;  ay_m -= 511;

 az_m -= 511;

 if (debug) {      Serial.print("ax: ");    Serial.print(ax_m);    Serial.print("ay: ");    Serial.print(ay_m);    Serial.print("az: ");    Serial.println(az_m);  }

 //The real purpose of this method is to convert the accelometer values into usable angle values.    //Knuckles904 pointed me to this app note: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf  //That paper gives the math for implementing a tilt sensor using 3-axis accelerometers.  Roll(X) and pitch(Y) are directly  //applicable.  Yaw(Z) is not since there is no 'tilt' with respect to the earth.      //Once the accelerometer values have been biased to zero (by subtracting 511 above), then they should fall in a range from  //-512 to +511.  double x = angleInRadians(-512, 511, ax_m);  double y = angleInRadians(-512, 511, ay_m);  double z = angleInRadians(-512, 511, 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);  }  

Back to top  

#include <AXELCD.h>

#include <MD23.h>#include <Wire.h>

#include <math.h>

/////////////////////////////////////////#define NUMREADINGS 5 //Gyro noise filterint readings[NUMREADINGS];                // the readings from the analog input (gyro)int index = 0;                            // the index of the current readingint total = 0;                            // the running totalint average = 0;                          // the average

int inputPin =0;                          //Gyro Analog input///////////////////////////////////////

float dt = .1; // .06; //( 1024.0 * 256.0 ) / 16000000.0; (Kalman)  // what does this means ?? hand tunedint mydt = 20; //in ms.static float PP[2][2] = { //(Kalman)  {    1, 0   }  , //(Kalman)  {    0, 1   }  ,//(Kalman)}; //(Kalman)

/* * Our two states, the angle and the gyro bias.  As a byproduct of computing * the angle, we also havef an unbiased angular rate available.   These are * read-only to the user of the module. */float angle = 0.0; //(Kalman)float q_bias = 0.0; //(Kalman)float rate = 0.0; //(Kalman)float                   q_m = 0.0; //(Kalman)

int ax_m=0;int ay_m=0;int cnt = 0;            //Counterunsigned long lastread=0;unsigned long startmillis=0;

/* * R represents the measurement covariance noise.  In this case, * it is a 1x1 matrix that says that we expect 0.3 rad jitter * from the accelerometer. */float R_angle = .3; //.3 deafault, but is adjusted externally (Kalman)

/* * Q is a 2x2 matrix that represents the process covariance noise. * In this case, it indicates how much we trust the acceleromter * relative to the gyros.

*/static const float Q_angle = 0.001; //(Kalman)static const float Q_gyro = 0.003; //(Kalman)int ledPin = 13;

MD23 md23;AXELCD lcd;

byte displayState = 0; // 0 clean,float oldAngle = 0.0; //

float P = 0.0;float I = 0.0;float D = 0.0;

void setup(){  pinMode(ledPin, OUTPUT);  pinMode(2, INPUT);     // declare pushbutton as input

 delay(500);  Wire.begin(); // init i2c  md23.setMode(0);  // set the mode (0 is default, so not really useful :P)  md23.disableSpeedRegulation();  lcd.clearDisplay(); // this always need 30 millis delay.  delay(30);  lcd.write("Chiappe!");  delay(10);  lcd.moveLine2(0);  delay(10);  lcd.write("Battery:"); // every other command needs 10 millis delay  delay(10);

 byte b = md23.getBatteryVolts();  lcd.write(b, DEC);  delay(10);  lcd.moveLine2(0);  delay(10);

 Serial.begin(9600);  for (int i = 0; i < NUMREADINGS; i++)    readings[i] = 0;                      // initialize all the readings to 0 (gyro average filter)

 startmillis = millis();

 }float sqr(float a){  return a*a;  }float sgn (float a)    {    if (a > 0)        return +1;    else        if (a < 0)            return -1;    else        return 0;}

long readMAX127(byte chan){  byte control = 0x80 + (chan << 4);  byte addr = 0x28;

 Wire.beginTransmission(addr);  Wire.send(control); // send value  Wire.endTransmission();

 Wire.requestFrom(addr, (byte)2);

 byte b1 = Wire.receive();  byte b2 = Wire.receive() >> 4;  long data = (b1 << 4) | (b2 & 0x0F);

 return data;}

void loop(){  int delta = millis()-lastread;  if( delta >= mydt) { // sample every dt ms -> 1000/dt hz.    lastread = millis();        total -= readings[index];               // subtract the last gyro reading    readings[index] = analogRead(2); // read from the gyro sensor    total += readings[index];               // add the reading to the total    index = (index + 1);                    // advance to the next index

   if (index >= NUMREADINGS)               // if we're at the end of the array...      index = 0;                            // ...wrap around to the beginning

   average = (total / NUMREADINGS)-500;    // calculate the average of gyro input

   dt = ((float)delta) / 1000.0;

   q_m= ((float)average)*(1500.0/1024.0)*PI/180 ;  // HAC remove 1.5 mult

   /* Unbias our gyro */    const float q = q_m - q_bias; //(Kalman)

   const float Pdot[2 * 2] = {      Q_angle - PP[0][1] - PP[1][0], /* 0,0 */ //(Kalman)      - PP[1][1], /* 0,1 */      - PP[1][1], /* 1,0 */      Q_gyro /* 1,1 */    };

   /* Store our unbias gyro estimate */    rate = q; //(Kalman)

   /** Update our angle estimate     * angle += angle_dot * dt     *       += (gyro - gyro_bias) * dt     *       += q * dt     */    angle += q * dt; //(Kalman)

   /* Update the covariance matrix */    PP[0][0] += Pdot[0] * dt; //(Kalman)    PP[0][1] += Pdot[1] * dt; //(Kalman)    PP[1][0] += Pdot[2] * dt; //(Kalman)    PP[1][1] += Pdot[3] * dt; //(Kalman)

   // read here!    ax_m = analogRead(0)-510;    ay_m = analogRead(1)-510;

   // non aggiusto R    // R_angle= (joy_y_axis+1)*0.0098039; //external adjust jitter of accelerometer with nunchuck joystick

   const float angle_m = atan2( ay_m, ax_m ); //(Kalman)    const float angle_err = angle_m - angle; //(Kalman)    const float C_0 = 1; //(Kalman)    const float PCt_0 = C_0 * PP[0][0];  //(Kalman)    const float PCt_1 = C_0 * PP[1][0]; //(Kalman)    const float E =R_angle+ C_0 * PCt_0; //(Kalman)    const float K_0 = PCt_0 / E; //(Kalman)    const float K_1 = PCt_1 / E; //(Kalman)    const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] (Kalman) */

   const float t_1 = C_0 * PP[0][1]; /* + C_1 * P[1][1]  = 0 (Kalman) */

   PP[0][0] -= K_0 * t_0; //(Kalman)    PP[0][1] -= K_0 * t_1; //(Kalman)    PP[1][0] -= K_1 * t_0; //(Kalman)    PP[1][1] -= K_1 * t_1; //(Kalman)    angle += K_0 * angle_err; //(Kalman)    q_bias += K_1 * angle_err; //(Kalman)

 float calibration = -4.5; // read from a pot or something  float myangle=(angle*57.2957795130823)-90.0 + calibration;

 if ((millis()-startmillis) > 6000 )  {    digitalWrite(ledPin, HIGH);

//    float integralAverage = (integral / NUMINTEGRALS);//    integral = integral - integralAverage;

 int val = digitalRead(2);  // read input value  if (val == HIGH) {     I = 0.0;    }    

   float rate_deg = rate * 57.2957795130823;    float pot1 = ((float)readMAX127(1))  ;    float pot2 = ((float)readMAX127(2))  ;    float pot3 = ((float)readMAX127(3))  ;        // myangle is up at 0    // motors is stopped at 128        P = (myangle * (pot1 / 100.0)); // pot/10    D = (myangle-oldAngle) * (pot2 / 10.0); // Kd = 50 (800 at pot)

   I = I + myangle * (pot3/100.0); // I * 1.0 + myangle * pot;  // 123 looks good        float pid = P+I+D;    float motors = 128.0 - (pid+(sgn(pid)*sqr(pid/18)));    // cap the value    if (motors>250.0) motors = 250.0;    if (motors<5.0) motors = 5.0;        byte motorsInput = (byte)motors;    //Serial.println(myangle, DEC);    Serial.println(motorsInput, DEC);    if ((myangle > 80) || (myangle < -80))    {      md23.setMotor1Speed(128);          md23.setMotor2Speed(128);      I=0; // avoid accumulation of integration when stopped    }    else    {      md23.setMotor1Speed(motorsInput);          md23.setMotor2Speed(motorsInput);    }    

   oldAngle = myangle;        

       // update the display    displayState ++;    if (displayState==7) displayState = 0;    switch (displayState)    {      case 0:      lcd.moveLine2(0);      break;      case 1:      lcd.write(pot1, DEC);      break;      case 2:      lcd.write(" ");      break;      case 3:      lcd.write(pot2, DEC);      break;      case 4:      lcd.write(" ");      break;      case 5:

     lcd.write(pot3, DEC);      break;      case 6:      lcd.write("    ");      break;    }      }  else  {    digitalWrite(ledPin, LOW);  }

//    Serial.print(" ");    //    Serial.println(int(angle_m*57.295779513082)+180, DEC); //Prints the accelometer//    Serial.println(ay_m, DEC); //Prints the accelometer

//    Serial.print(" ");     //Prints degrees Acceleromter+Gyros+KalmanFilters//    Serial.print(" ");  }

}

This is the update

/ Modified Kalman code using Roll, Pitch, and Yaw from a Wii MotionPlus and X, Y, and Z accelerometers from a Nunchuck.// Also uses "new" style Init to provide unencrypted data from the Nunchuck to avoid the XOR on each byte.// Requires the WM+ and Nunchuck to be connected to Arduino pins D3/D4 using the schematic from Knuckles904 and johnnyonthespot// See http://randomhacksofboredom.blogspot.com/2009/07/motion-plus-and-nunchuck-together-on.html// Kalman Code By Tom Pycke. http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data// Original zipped source with very instructive comments: http://tom.pycke.be/file_download/4//// Some of this code came via contributions or influences by Knuckles904, Ed Simmons, Jordi Munoz, and Adrian Carter//

// Created by Duckhead v0.3 Nunchuk accelerometers to angles is still not quite right

#include <math.h>#include <Wire.h>/////////////////////////////////////////

struct GyroKalman{ /* These variables represent our state matrix x */ float x_angle,

x_bias;

/* Our error covariance matrix */ float P_00,

P_01, P_10, P_11;

/* * Q is a 2x2 matrix of the covariance. Because we * assume the gyro and accelerometer noise to be independent * of each other, the covariances on the / diagonal are 0. * * Covariance Q, the process noise, from the assumption * x = F x + B u + w * with w having a normal distribution with covariance Q. * (covariance = E[ (X - E[X])*(X - E[X])' ] * We assume is linear with dt */ float Q_angle, Q_gyro;

/* * Covariance R, our observation noise (from the accelerometer) * Also assumed to be linair with dt */ float R_angle;};

struct GyroKalman rollData;struct GyroKalman pitchData;struct GyroKalman yawData;

// test for 3 axisint readingsX;int readingsY;int readingsZ; // the readings// End of 3 axis stuffint index = 0; // the index of the current readingint inputPin =0; // Gyro Analog input//WM+ stuffbyte data[6]; //six data bytesint yaw = 0;int pitch = 0;int roll = 0; //three axesint yaw0 = 0;int pitch0 = 0;int roll0 = 0; //calibration zeroesbool slowYaw;bool slowPitch;

bool slowRoll;

//Precomputed constants to avoid floating point division at runtime (too many clock cycles)static const double RadianToDegree = 57.2957795; // 180/PIstatic const double DegreeToRadian = 0.0174532925; // PI/180

//The WMP has two modes: 2000 deg/s (slow) and 500 deg/s (fast). It is assumed that the sensitivity of the output follows//the same ratio (it seems that the sensitivity is 20 mV/deg/s and 0.5 mV/deg/s), which is to say it's proportional to 4:1.static const int wmpSlowToDegreePerSec = 20;static const int wmpFastToDegreePerSec = wmpSlowToDegreePerSec/5;

///////////////////////////////////////

float accelAngleX=0; //NunChuck X anglefloat accelAngleY=0; //NunChuck Y anglefloat accelAngleZ=0; //NunChuck Z anglebyte outbuf[6]; // array to store arduino outputint cnt = 0; //Counterunsigned long lastread=0;

/* * R represents the measurement covariance noise. In this case, * it is a 1x1 matrix that says that we expect 0.3 rad jitter * from the accelerometer. */static const float R_angle = .3; //.3 default

/* * Q is a 2x2 matrix that represents the process covariance noise. * In this case, it indicates how much we trust the acceleromter * relative to the gyros. */static const float Q_angle = 0.01; //(Kalman)static const float Q_gyro = 0.4; //(Kalman)

void switchtonunchuck(){/* PORTE ^= 32; // Toggle D3 LOW PORTG |= 32; // Force D4 HIGH */ // Fast switching of pins... Arduino MEGA specific... digitalWrite(3, LOW); digitalWrite(4, LOW); digitalWrite(4, HIGH);

}

void switchtowmp(){/* PORTG ^= 32; // Toggle D4 LOW PORTE |= 32; // Force D3 HIGH */ // Fast switching of pins... Arduino MEGA Specific pin maps, so switched back to 'normal' code for example digitalWrite(3, LOW); digitalWrite(4, LOW); digitalWrite(3, HIGH);}

void nunchuck_init () //(nunchuck){ // Uses New style init - no longer encrypted so no need to XOR bytes later... might save some cycles

Wire.beginTransmission (0x52); // transmit to device 0x52 Wire.send (0xF0); // sends memory address Wire.send (0x55); // sends sent a zero. Wire.endTransmission (); // stop transmitting delay(100); Wire.beginTransmission (0x52); // transmit to device 0x52 Wire.send (0xFB); // sends memory address Wire.send (0x00); // sends sent a zero. Wire.endTransmission (); // stop transmitting}

void setup(){ Serial.begin(115200);/* DDRG |= 32; // Force D3 to Output // Arduino MEGA "fast" pin config. Reverted to 'normal' for example post. DDRE |= 32; // Force D4 to Output PORTG ^= 32; // Toggle D3 HIGH PORTE ^= 32; // Toggle D4 HIGH */

pinMode(3, OUTPUT); pinMode(4, OUTPUT); digitalWrite(3, HIGH); digitalWrite(4, HIGH);

Wire.begin (); // join i2c bus with address 0x52 (nunchuck) switchtowmp(); wmpOn(); calibrateZeroes(); switchtonunchuck(); nunchuck_init (); // send the initilization handshake

initGyroKalman(&rollData, Q_angle, Q_gyro, R_angle); initGyroKalman(&pitchData, Q_angle, Q_gyro, R_angle); initGyroKalman(&yawData, Q_angle, Q_gyro, R_angle);

lastread = millis();}

void initGyroKalman(struct GyroKalman *kalman, const float Q_angle, const float Q_gyro, const float R_angle) { kalman->Q_angle = Q_angle; kalman->Q_gyro = Q_gyro; kalman->R_angle = R_angle;

kalman->P_00 = 0; kalman->P_01 = 0; kalman->P_10 = 0; kalman->P_11 = 0;}

void send_zero () //(nunchuck){ Wire.beginTransmission (0x52); // transmit to device 0x52 (nunchuck) Wire.send (0x00); // sends one byte (nunchuck) Wire.endTransmission (); // stop transmitting (nunchuck)}

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.endTransmission();}

void wmpSendZero(){ Wire.beginTransmission(0x52);//now at address 0x52 Wire.send(0x00); //send zero to signal we want info Wire.endTransmission();}

void calibrateZeroes(){ long y0 = 0; long p0 = 0; long r0 = 0; const int avg = 100; for (int i=0;i<avg; i++){ wmpSendZero(); Wire.requestFrom(0x52,6); for (int t=0;t<6;t++){

data[t]=Wire.receive(); } y0+=(((data[3] >> 2) << 8)+data[0]); r0+=(((data[4] >> 2) << 8)+data[1]); p0+=(((data[5] >> 2) << 8)+data[2]); }

yaw0 = y0/avg; roll0 = r0/avg; pitch0 = p0/avg;}

void receiveData(){ receiveRaw(); //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus //for info on what each byte represents yaw -= yaw0; roll -= roll0; pitch -= -pitch0;}

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]; roll=((data[4] >> 2) << 8)+data[1]; pitch=((data[5] >> 2) << 8)+data[2];

slowPitch = data[3] & 1; slowYaw = data[3] & 2; slowRoll = data[4] & 2;

}

void loop(){ unsigned long now = millis(); float dt = ((float)(now - lastread)) * 0.001; //compute the time delta since last iteration, in seconds.

if (dt >= 0.01) { //Only process delta angles if at least 1/100 of a second has elapsed lastread = now;

switchtowmp(); receiveData(); switchtonunchuck(); readNunchuck();

// Handle the fast/slow bits of the Wii MotionPlus const int rollScale = slowRoll ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec; const int pitchScale = slowPitch ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec; const int yawScale = slowYaw ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;

readingsX = (roll/rollScale); // read from the gyro sensor readingsY = (pitch/pitchScale); // read from the gyro sensor readingsZ = (yaw/yawScale); // read from the gyro sensor

float tmpX = accelAngleX * DegreeToRadian; float tmpY = accelAngleY * DegreeToRadian; float tmpZ = accelAngleZ * DegreeToRadian;

predict(&rollData, readingsX * DegreeToRadian, dt); predict(&pitchData, readingsY * DegreeToRadian, dt); predict(&yawData, readingsZ * DegreeToRadian, dt);

float rollAngle = update(&rollData, tmpX); float pitchAngle = update(&pitchData, tmpY); float yawAngle = update(&yawData, tmpZ);

Serial.print("Roll: "); Serial.print(rollAngle*RadianToDegree); Serial.print(" Pitch: "); Serial.print(pitchAngle*RadianToDegree); Serial.print(" Yaw: "); Serial.println(yawAngle*RadianToDegree); }}//Stitch here