Gyro & accelerometers

To reduce the costs further I wanted to replace the piggy back print housing the gyro and accelometer sensors on both the Elektor and Zzaag designs. This is available from fun components and is called the SEN01 sensorboard. They cost 75.50€ and have a 2 axis gyro and three axis accelerometer of which 2 axis are actually wired to the processor board.

There are four possibilities:
1) The Wii Motion plus has a three axis gyroscope and the Nunchuk has a three axis accelerometer. These components can be bought separately and cost around 20€ each. Later on i will explain how these can be used and connected.
2) Another solution is to use one of the two ready made boards below. The board below on the left from www.DroTek.fr costs 41.90€ so is on the same price level as the Wii solution but additionally has a HMC5883 magnetometer which allows stabilisation of the yaw rotation which can not be achieved with only gyro and accelerometer.
3) Another very nice IMU is the one that can be found on FreeIMU. This has been developed by Fabio Varesano and is now at version 0.3.5. I recommend the version that has the BMP085 pressure sensor in addition to the BMA180 accelerometer, ITG3200 gyro and the HMC5883L magnetometer (the HMC5883L version is an improved one over the HMC5883). The pressure sensor is used in helicoptor flight control but is not required for the Seguino. Eagle files BOM and gerbers can be found on Fabio’s site.
Ready made boards with all components mounted can be bought from www.viacopter.eu for 79.00€
4) Make your own print using the different components which you can buy below.
www.DroTek.fr HMC5883L 5,90€ , ITG3200 15,90€ and the BMA180 12,90€. Finally the BMP085 costs 7,90€.
www.viacopter.eu HMC5883L 6.00€ , ITG3200 15.00€ and the BMA180 10.00€. Finally the BMP085 costs 10.00€.
www.lipoly.de HMC5883L 3.90€, ITG3200 14.90€ and the BMA180 12.82€. Finally the BMP085 costs 6,79€.
This last site will also sell the MPU-6000 and MPU-6050 at a price of 24.90€ when they become available.
The MPU-6000™ family provides the world’s first integrated 6-axis motion processor solution that eliminates the package-level gyro/accel cross-axis misalignment associated with discrete solutions. The devices combine a 3-axis gyroscope and a 3-axis accelerometer on the same silicon die together with an onboard Digital Motion Processor™ (DMP™) capable of processing complex 9-axis sensor fusion algorithms. The parts’ integrated 9-axis sensor fusion algorithms access external magnetometers or other sensors through an auxiliary master I2C bus, allowing the devices to gather a full set of sensor data without intervention from the system processor. The devices are offered in the same 4x4x0.9 mm QFN footprint and pinout as the current MPU-3000™ family of integrated 3-axis gyroscopes, providing a simple upgrade path and making it easy to fit on space constrained boards.
If you look at the price of the sensors that go into the DroTek board you are already at 30.00€ so that passive components plus board only cost 10.00€ which is a good price and avoids the hassle of making your own board.
If you want the BMP085 pressure sensor you can use the viacopter print but remember that this is not really needed for the seguino.

So by selecting the any of these solutions you save 35€ and in addition have 3 axis on both the accelerometer and the gyro.
I finally chose the Wii route so below you can find a description of how to connect and use the Wiimotion plus and Nunchuck.

segwaydroteksegwayfreeimu

The Wii Motion Plus contains two gyro sensors: A dual-axis gyro by InvenSense, the IDG-600 (pitch and roll), and a single-axis gyro by EPSON TOYOCOM labelled X3500W (yaw). The combined use of a dual-axis gyro and a single-axis gyro is what enables the Wii Motion Plus to report the angular rate in all three axis, thus allowing full orientation tracking. Specifications of the X3500W are currently unknown, but may be similar to the specifications of the similarly named XV-3500CB (same manufacturer.) The Wii Motion Plus contains RVL-GU chip and EEPROM. RVL-GU chip is a NEC 78K0/KC2 microprocessor.

The best place to learn about the Nunchuk and Wii Motion Plus is on the following link http://wiibrew.org/wiki/Wiimote/Extension_Controllers. Here you will find all sorts of information regarding these two items.
It is possible to read from both Wii Motion Plus and the Nunchuk at the same time by activating the Wii MotionPlus in one of the pass-through modes suggested by DogP. When activated, this interleaves Nunchuk data with Wii MotionPlus data on every other read. Data passing through are modified to make room for 3 bookkeeping bits.  Below you can see the differences between normal mode and passthrough mode.

Wii with Wii Motion Plus and Nunchuck in Normal  mode:                                                              Wii with Wii Motion Plus and Nunchuck in Passthrough mode:

wmplusnormalwmpluspassthrough

Wii with Wii Motion Plus and Nunchuck in Passthrough mode:

wmplusnunchuckpassthrough

 

Below is a detailed photo of the Nunchuk and of the Wii Motion Plus showing also the connectors.

nunchuckpcbfrontnunchuckpcbrearWii_ConnectorWMPmaleconnector

Here you can see the connections of the nunchuck.                                                                                             Nunchuck connector                                                             Wii Motion Plus connector (Nunchuck)
The connector view is looking at the connector towards the wires (see picture right)

|~~_____~~|
|  2 4 6  |
|  -----  |
|  1 3 5  |
 ---------
Pin Cable color Description
1 Red 3.3V
2 Yellow SCL. I²C Serial Clock. (400 kHz)
3 X Device select
4 X
5 Green SDA. I²C Serial Data
6 White Ground
wmplusphoto1wmplusphoto2

This is the  Wiimotionplus connector view is looking at the connector towards the wires.

|~~_____~~|
|  2 4 6  |
|  -----  |
|  1 3 5  |
 ---------
Pin Cable color Description
1 Gray 3.3V
2 Red SCL. I²C Serial Clock. (400 kHz)
3 White
4 Yellow
5 Green SDA. I²C Serial Data
6 Blue Ground

The Wii Motion Plus starts off at register 0xA60000 = 0x53 instead of register 0xA40000 = 0x52 (A6 = 1010 011x where x=read or write 0/1 right shift this and you get 0101 0011 = 0x53) which the Nunchuk uses, because it has its own extension port on the back allowing a Nunchuk for example to be plugged in along with the Wii Motion Plus. BUT it changes to 0xA40000 once it is activated by writing 05 to 0xA600FE. The extension is initialised by writing 0x55 to 0xA600F0.

While the Wii Motion Plus is still, the values will be about 0x1F7F (8,063), although it is best to calibrate for a few seconds every time you start, to get the actual zero values. 20 represents turning at about 1 degree per second. So divide by 20 to get the degrees per second.
At high speed (Low speed bit = 0) 20 represents turning at about 5 degree per second. So divide by 4 to get the degrees per second.
The gyros are slightly noisy, so you should assume any speed less than 0.5 degrees per second is actually zero.
Yaw/Roll/Pitch fast/slow bits are 1 when the Wii Motion Plus is rotating slowly (or not rotating at all), 0 when the Wii Motion Plus is rotating fast.
Extension connected is 1 when an extension is connected to the Wii Motion Plus extension port.
Note: The definition of yaw, pitch, and roll, are slightly different for the gyros and accelerometers. Accelerometers measure angles relative to gravity. Gyros measure angles relative to the Wii Motion Plus. If the Wii Motion Plus is up the right way, then they are the same. But if the Wii Remote is on its side, then what the gyros call “pitch” is actually what the accelerometers would call “yaw”. This is why the relative orientation between the Wii Motion Plus and the Nunchuk is important. If you do not want to change then make your assembly the same as mine.

wmplusandnunchuckpcbwmplusorientationwmpguts

The connections are shown on the drawings and hopefully do not need a further explantion. I have seen power connecting colours on the Wii Motion Plus which are brown or grey. In all cases this was pin7.

Now for the software. So far the Wii Motion Plus and Nunchuk can talk to the board and a Kalman filter allows stable reading on the Roll and Pitch angles.
These are printed out to the PC so you can check their accuracy. Seems very good and stable to me for what its worth. The program below displays pitch, roll and yaw angles based on kalman filter technology. Obviously the yaw reading is not relevant and I was thinking of using the pitch data to maybe aid cornering but otherwise also no meaning. Still the angle seems to be stable and usable for Wheelie purposes

Show »

// ............................................................................................................................
// Seguino balance part v1.00 dd 06-09-2010
// by Krulkip
// 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.
// Uses the passthrough functionality of the Nunchuck connected to the WMplus
// 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
// The Axis definitions: x-axis is always pointing forward, the y-axis points to the right, and the z-axis points down.
// Positive pitch: nose up ----- Positive roll: right wing up ----- Positiv yaw: anticlockwise
// TO DO: normally roll & yaw should be other way
// 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
// Code for reading data from a Wii Motion plus device connected to a Nunchuck
// and calculating roll and pitch Angle relative to earth plane in degrees. 
// Angle is stabilised using kalman filter
// Links to other sites used in this example
// 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.
// http://obex.parallax.com/objects/471/   .... ideas for passthrough thanks to DogP
// 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--, --6--, --5--, --4--, --3--, --2--, --1--, --0--
// 1  SY --7--, --6--, --5--, --4--, --3--, --2--, --1--, --0--
// 2  AX --9--, --8--, --7--, --6--, --5--, --4--, --3--, --2--
// 3  AY --9--, --8--, --7--, --6--, --5--, --4--, --3--, --2--
// 4  AZ --9--, --8--, --7--, --6--, --5--, --4--, --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) or motion plus  (1 0) detection.
// Byte 4 Bit 0 which is the extention controller detection bit. (1 = extension present)
// Hardware Arduino with ATMega328
// Connections SDA to AD4 (Analog4) and SCL to AD5 (Analog5)
//........................................................................
#include <Wire.h>
struct GyroKalman
{
  double x_angle, x_bias;      // These variables represent our state matrix x 
  double P_00,P_01,P_10,P_11;  // Our error covariance matrix
   /* 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;
  double R_angle;               // Covariance R, our observation noise (from the accelerometer) Also assumed to be linear with dt
};
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/PI
  static 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 = 16;
  static const int wmpFastToDegreePerSec = wmpSlowToDegreePerSec/4;
// Hardware identifiers. Polling data[5]&0x03 for present device
  static const int NC = 0x00;
  static const int MP = 0x02;
  int currentDevice;
//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 axis
  struct GyroKalman rollData;  struct GyroKalman pitchData;  struct GyroKalman yawData;
  struct RungeKutta yawRK;
  int readingsX, readingsY, readingsZ;      // raw 3 axis gyro readings
  int xID;
  int sensorsign[]={1,-1,-1,-1,-1,1};
  byte data[6]; //six data bytes
  int ax_m = 0, ay_m = 0, az_m = 0;
  long yaw = 0,pitch = 0,roll = 0;          // normalized (scaled per WM+ mode) rotation values
  long yaw0 = 0, pitch0 = 0, roll0 = 0;     // calibration zeroes
  long xAcc0 = 0, yAcc0 = 0, zAcc0 = 0;
  boolean slowYaw, slowPitch, slowRoll;     // WM+ state variables - if true, then in slow (high res) mode
  boolean debug = false;
  float accelAngleX=0;	//NunChuck X angle
  float accelAngleY=0;	//NunChuck Y angle
  float accelAngleZ=0;	//NunChuck Z angle
  unsigned long lastread=0; // Internal program state variables last system clock in millis

void
setup ()
{
  Serial.begin (19200);
  Wire.begin();
//  Next three lines not needed. I used to experiment with high speed TWI
  #define TWI_FREQ_NUNCHUCK 100000L
  #define CPU_FREQ 16000000L
  TWBR = ((CPU_FREQ / TWI_FREQ_NUNCHUCK) - 16) / 2;
  Serial.print ("Finished setupn"); Serial.print ("Now detecting WM+n");
  delay(100);
// now make Wii Motion plus the active extension
// nunchuk mode = 05, wm+ only = 04, classic controller = 07
  Serial.print ("Making Wii Motion plus the active extension in nunchuk mode = 05 ........");
  Wire.beginTransmission(0x53); Wire.send(0xFE); Wire.send(0x05); Wire.endTransmission(); // nunchuk
  Serial.print (" OK done"); Serial.print ("rn");
  delay (100);
// now innitiate Wii Motion plus  
  Serial.print ("Innitialising Wii Motion plus ........");
  Wire.beginTransmission(0x53); Wire.send(0xF0); Wire.send(0x55); Wire.endTransmission();
  Serial.print (" OK done"); Serial.print ("rn");
  delay (100);
  Serial.print ("Set reading address at 0xFA .......");
  Wire.beginTransmission(0x52); Wire.send(0xFA); Wire.endTransmission();
  Serial.print(" OK done"); Serial.print ("rn");
  delay (100);
  Wire.requestFrom (0x52,6);
  for (int t=0;t<6;t++) { data[t] = Wire.receive();Serial.print(data[t],HEX);Serial.print(" ");  }
  Serial.print ("rn");
  xID= data[0] + data[1] + data[2] + data[3] + data[4] + data[5];
  Serial.print("Extension controller xID = 0x");
  Serial.print(xID,HEX);
  if (xID == 0xCB) { Serial.print (" Wii Motion plus connected but not activated"); }
  if (xID == 0xCE) { Serial.print (" Wii Motion plus connected and activated"); }
  if (xID == 0x00) { Serial.print (" Wii Motion plus not connected"); } Serial.print ("rn");
  delay (100);
   // Now we want to point the read adress to 0xa40008 where the 6 byte data is stored 
  Serial.print ("Set reading address at 0x08 .........");
  Wire.beginTransmission(0x52); Wire.send(0x08); Wire.endTransmission();
  Serial.print(" OK done"); Serial.print ("rn");
    // check if nunchuk is really connected
  if ((data[4]&0x01)==0x01) {
  Serial.print("Nunchuck connected: "); Serial.print ("rn");
  }
  calibrateZeroes();
  Serial.print("Calibrated zeros: "); Serial.print ("rn");
  Serial.print (" roll0 = "); Serial.print (roll0,HEX);
  Serial.print (" pitch0 ="); Serial.print (pitch0, HEX);
  Serial.print (" yaw0 = "); Serial.print (yaw0,HEX);
  Serial.print (" xAcc0 = "); Serial.print (xAcc0);
  Serial.print (" yAcc0 = "); Serial.print (yAcc0);
  Serial.print (" zAcc0 = "); Serial.print (zAcc0); Serial.print ("rn");
  delay(2000);
  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_3 = 0;  yawRK.val_i_2 = 0;  yawRK.val_i_1 = 0;  yawRK.previous = 0;
  lastread = millis ();
} 

void
send_zero ()
{
    Wire.beginTransmission(0x52); Wire.send(0x00); Wire.endTransmission();
}
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 data angles if at least 1/100 of a second has elapsed
    lastread = now;
// now follows conversion command instructing extension controller to get
// all sensor data and put them into 6 byte register within the extension controller
    receiveData();
    receiveData();
// 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);
//    predict(&yawData, readingsZ * DegreeToRadian, dt);  
    double  rollAngle  = update(&rollData, accelAngleX);
    double  pitchAngle = update(&pitchData, accelAngleY);
//    double  yawAngle   = update(&yawData, accelAngleZ);
    double  yawAngle = computeRK4(&yawRK, readingsZ*DegreeToRadian*dt);
// use Runge Kutta4 instead of Kalman for Yaw. Its better but not perfect
    Serial.print(" ..... Roll: "); Serial.print(rollAngle*RadianToDegree); Serial.print(", ");
    Serial.print(" Pitch: "); Serial.print(pitchAngle*RadianToDegree); Serial.print(", ");
    Serial.print(" Yaw: "); Serial.println(yawAngle*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;
}

void receiveData(){
  rawreceivedata();
  if  ((data[5]&0x03)==MP) { yaw-= yaw0; roll -= roll0; pitch -=pitch0;}
  // Remove callibration values from nunchuk accelerometer to zero the values.
  // The execution of this function results in the updating of 3 globals, accelAngleX, accelAngleY, and accelAngleZ with the angle in radians.
  if  ((data[5]&0x03)==NC) { ax_m -= xAcc0; ay_m -= yAcc0; az_m -= zAcc0; az_m += 211; }
  // Dont know why this is needed. My zero z value is 720 ie gravity

  if ((debug) && ((data[5]&0x03)==MP)) {
  Serial.print("t Gyro yaw: t"); Serial.print(yaw);
  Serial.print("t Gyro roll: t"); Serial.print(roll);
  Serial.print("t Gyro Pitch: t"); Serial.print(pitch);
  }
  if ((debug) && ((data[5]&0x03)==NC)) {
  Serial.print("t ax_m: t"); Serial.print(ax_m);
  Serial.print("t ay_m: t"); Serial.print(ay_m);
  Serial.print("t az_m: t"); Serial.print(az_m);
  }

// The real purpose of this method is to convert the accelometer values into usable angle values.  
// see 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 calibration value above), then they should fall in a range from
// -512 to +511.
    double x = angleInRadians(-512, 511, ax_m), y = angleInRadians(-512, 511, ay_m), 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 = (accelAngleX * 0.9 + atan(x / sqrt(ySquared + zSquared))*0.1);
    accelAngleY = (accelAngleY * 0.9 + atan(y / sqrt(xSquared + zSquared))*0.1);
    accelAngleZ = (accelAngleZ * 0.9 + atan(sqrt(xSquared + ySquared)/ z)*0.1);
// filter readings using low pass filter for acceleromters to remove jitter
// big percentage of previous value plus smaller percentage of current value
// effect is delay large changes from reaching the output but at cost of reduced sensitivity 
}

/* 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, long measured) {
  float x = (hi - lo);
  return (float)(measured/x) * PI;
}

/*
 * 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 calibrateZeroes() {
  long y0 = 0;
  long p0 = 0;
  long r0 = 0;
  long xa0 = 0;
  long ya0 = 0;
  long za0 = 0;
  int avg = 300;
  for (int i=0;i<avg;i++) {
  rawreceivedata();
  if ((data[5]&0x03)==MP) { y0 +=yaw;r0 +=roll; p0 +=pitch; }
  if ((data[5]&0x03)==NC) { xa0 +=ax_m; ya0 += ay_m; za0 += az_m; }
  }
  yaw0=y0/(avg/2); roll0=r0/(avg/2); pitch0=p0/(avg/2);
  xAcc0 = xa0/(avg/2); yAcc0 = ya0/(avg/2); zAcc0 = za0/(avg/2);
  Serial.print ("rn");
}  

void rawreceivedata() {
  send_zero(); //send zero before each request (same as nunchuck)
  delay(10);
  Wire.requestFrom(0x52,6);//request six bytes from the WM+
  for (int i=0;i<6;i++){
    data[i]=Wire.receive();
  }
  if  ((data[5]&0x03)==MP) {

  yaw=    (sensorsign [0]-1) * -512 + (sensorsign[0] * ((data[3]>>2)<<8) | data[0]);
  roll =  (sensorsign [1]-1) * -512 + (sensorsign[1] * ((data[4]>>2)<<8) | data[1]);
  pitch = (sensorsign [2]-1) * -512 + (sensorsign[2] * ((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   
}
  if ((data[5]&0x03)==NC) {
 /*
 * This function processes the byte array from the wii nunchuck.
 */
  ax_m = (sensorsign [3]-1) * -512 + (sensorsign[3] * (data[2] << 2)   | ((data[5] >> 3) & 0x02));
  ay_m = (sensorsign [4]-1) * -512 + (sensorsign[4] * (data[3] << 2)   | ((data[5] >> 4) & 0x02));
  az_m = (sensorsign [5]-1) * -512 + (sensorsign[5] * ((data[4]>>1)<<3)| ((data[5] >> 5) & 0x06)); 
}
}

Here you can download this file.

Leave a Reply