Wii Motion Plus on Arduino Problem in Calculation of Angle

Hi Everybody,

i attached a wii motion plus gyro sensor to my arduino uno.

the wiring is correct, i use the i2c protocoll and use pullups on clock and dataline to vcc.

it all runs on 3.3 V and nothing else is on the i2c bus.

my goal is to calculate the angle the sensor is in by integratin the angle speed over time.

i transmit the values via serial to a programm i wrote in processing to display the values i get.

i am measuring the angle of one axis only to make it more simple. but the problem is the same on all three axis.

here is my problem (i posted it on youtube):

sorry for the poor quality. please ignore anything but the black line / graph (actualy, there are 3 but the show all the same).

here is my problem:
as long as i move the wmp just around one axis everything is fine and when in turn the wmp back to horizontal line, the graph goes back to 0.
when start moving the wmp around on all three axis and a bit more fast, the angle just doesnt go back to 0.

i posted my code below.

here is what i tried to fix the problem but nothing makes a difference:

  • use pullups
  • dont use pullups
  • use 3.3V
  • use 5.0 V
  • differnt sample intervalls from 150 to 20.
  • smoothing the value with different numbers of reading to smooth the value from
  • transmitting to serial only every 10th time
  • i monitored the measurement and loop time, its all fine…

please help me guys, im really out of ideas here…

thanks!

dom

// WMP + BMA020 - First Tries in imu
// http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/35
// thx duckhead and knuckles904

#include <Wire.h>

#define numberOfReadings 10 //Number of Sensor Readings to be averaged

int sampleIntervall = 20;  //sampleIntervall in millis

byte wmpdata[6];          //six data bytes out of wmp
bool slowYaw;
static const int wmpSlowToDegreePerSec = 20;
static const int wmpFastToDegreePerSec = wmpSlowToDegreePerSec/5;
long yrp;  //three axes, values out of wmp
long yrp0;  //calibration zeroes for wmp
long yrpRead; //values out of wmp in degrees
int yrpReadings[numberOfReadings]; //buffer for calculating averages - first dimension=y
long yrpTotal; //total buffer for average calculation
float yrpAverageAngleSpeed; //the speed to be calculated from sensors, which is the Average
float yrpAngle; //the angles to be calculated from sensors
int yawScale;

int timeCalibration = 0;
unsigned long timeRead = 0;
int lastRead = 0;
int readTime = 0;
int index = 0; //Index for counting #s of averaging cycles
int index2 = 0;
    
void setup() {

  Wire.begin();

    wmpOn();                        //turn WM+ on

    timeCalibration = wmpCalibrateZeroes();              //calibrate zeroes

    Serial.begin(19200);

    //intialize yrpreadings matrix:
    for(int n=0;n<numberOfReadings;n++){

        yrpReadings[n] = 0;
    }
}


void loop() {
          
    if (timeRead-millis() >= sampleIntervall){ //if enough time passed get a new reading

      lastRead = millis() - timeRead; //calculate time passed since last read
      timeRead = millis(); //Log the Time when read 
      
        yrpTotal = yrpTotal - yrpReadings[index]; // substract last reading

            
      wmpReceiveData(); //WMP Read (real degrees)
      
        yrpReadings[index] = yrpRead; //Fill the Average Buffer Array with real degrees

        yrpTotal = yrpTotal + yrpReadings[index]; // add new reading to total

      
      index++;
      
      if(index >= numberOfReadings){
       index = 0; //setback index when last array field is filled
      }
      
      //Average Angle Speed Calculation WMP in Degrees:
       yrpAverageAngleSpeed = (yrpTotal/numberOfReadings);

         
      //Angles Calcutation:
        yrpAngle += 0.0174532925 * (yrpAverageAngleSpeed*(float(lastRead)/1000)); //multiply angle with time passed since last read in seconds and convert from degrees to radian: Angle*(PI/180) = Angle*0,0174532925 

      
    index2++; //this is hust the index for defining how often the values should be transmitted to serial
    if (index2 > 10) index2 = 0;
    
    readTime = (millis() - timeRead);
    
    } 
    
    if(index2 == 10){
      //Transmission to Serail (Values used in Processing):    
      for (int m=0;m<3;m++){
        Serial.print(1, DEC); //ignore
        Serial.print(";");
      }
      for (int m=0;m<3;m++){
        Serial.print(yrpAngle, DEC);
        Serial.print(";");
      }
      for (int m=0;m<3;m++){
        Serial.print(yrpAverageAngleSpeed, DEC);
        Serial.print(";");
      }      
      Serial.print(lastRead, DEC);
      Serial.print(";");
      Serial.print(readTime, DEC);
      Serial.print(";");
      Serial.print(timeCalibration, DEC);    
      Serial.print("]");
    }  
}


//WMP functions:
//-------------------------------------------

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 wmpReceiveData(){
  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++){
    wmpdata[i]=Wire.receive();
  }

  yrp=(((wmpdata[3]>>2)<<8)+wmpdata[0]);        //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus

  slowYaw = wmpdata[3] & 2; // Read if in fast or slow mode


  // Handle the fast/slow bits of the Wii MotionPlus
  yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;

  yrpRead = yrp/yawScale; // make degrees out of readings

  yrpRead = yrpRead-yrp0; // calibration

}

int wmpCalibrateZeroes(){
  int timeCalibration = int(millis());
  int numberOfAverageings = 200;
  for (int k=0;k<numberOfAverageings;k++){
    wmpSendZero();
    Wire.requestFrom(0x52,6);
    for (int j=0;j<6;j++){
      wmpdata[j]=Wire.receive();
    }
    yrp0+=(((wmpdata[3]>>2)<<8)+wmpdata[0]);        //average readings for each zero

 }

  yrp0 = (yrp0/numberOfAverageings)/wmpSlowToDegreePerSec;

 
  timeCalibration = int(millis()) - timeCalibration;
  return timeCalibration;
}

1) Serial.begin(19200); ==> use 115200 to minimize time consumption.

2) Why doing the math on the Arduino, Processing is much faster.

3) (float(timeread)/1000) ==> timeread * 0.001

-- are the raw measurements (before math) stable -> do they go to zero?

thanks rob!

  1. changed to 115200 (yes, definitly faster :wink: )

  2. i do the math in arduino because i need the angle in arduino itself later on!

  3. changed it to 0.001 * lastRead

However, there is not really a change to the behaviour.

i monitored the angle, arduino calculated and the angleSpeed i use to calculate the angle in arduino.

the angleSpeed goes back to 0 perfectly, you caN see it in the video below in the lower graph.

the angle itself still shows the strange behaviour when moving the sensor quickly or slowly in 3 dimensions.
when i move it slowly in one axis its fine.
when i move it slowly in all three axis its wrong again.

there has to be a reason for the misscalculation of the angle, but i have no idea :frowning:

cheers

dom

and one again the code…

// WMP + BMA020 - First Tries in imu
// http://randomhacksofboredom.blogspot.com/2009/06/wii-motion-plus-arduino-love.html
// http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1248889032/35
// thx duckhead and knuckles904

#include <Wire.h>

#define numberOfReadings 10 //Number of Sensor Readings to be averaged

int sampleIntervall = 20;  //sampleIntervall in millis

byte wmpdata[6];          //six data bytes out of wmp
bool slowYaw;
static const int wmpSlowToDegreePerSec = 20;
static const int wmpFastToDegreePerSec = wmpSlowToDegreePerSec/5;
long yrp;  //three axes, values out of wmp
long yrp0;  //calibration zeroes for wmp
long yrpRead; //values out of wmp in degrees
int yrpReadings[numberOfReadings]; //buffer for calculating averages - first dimension=y
long yrpTotal; //total buffer for average calculation
float yrpAverageAngleSpeed; //the speed to be calculated from sensors, which is the Average
float yrpAngle; //the angles to be calculated from sensors
int yawScale;

int timeCalibration = 0;
unsigned long timeRead = 0;
int lastRead = 0;
int readTime = 0;
int index = 0; //Index for counting #s of averaging cycles
int index2 = 0;
    
void setup() {

  Wire.begin();

    wmpOn();                        //turn WM+ on

    timeCalibration = wmpCalibrateZeroes();              //calibrate zeroes

    Serial.begin(115200);

    //intialize yrpreadings matrix:
    for(int n=0;n<numberOfReadings;n++){

        yrpReadings[n] = 0;
    }
}


void loop() {
          
    if (timeRead-millis() >= sampleIntervall){ //if enough time passed get a new reading

      lastRead = millis() - timeRead; //calculate time passed since last read
      timeRead = millis(); //Log the Time when read 
      
        yrpTotal = yrpTotal - yrpReadings[index]; // substract last reading

            
      wmpReceiveData(); //WMP Read (real degrees)
      
        yrpReadings[index] = yrpRead; //Fill the Average Buffer Array with real degrees

        yrpTotal = yrpTotal + yrpReadings[index]; // add new reading to total

      
      index++;
      
      if(index >= numberOfReadings){
       index = 0; //setback index when last array field is filled
      }
      
      //Average Angle Speed Calculation WMP in Degrees:
       yrpAverageAngleSpeed = (yrpTotal/numberOfReadings);

         
      //Angles Calcutation:
        yrpAngle += 0.0174532925 * (yrpAverageAngleSpeed * lastRead * 0.001 ); //multiply angle with time passed since last read in seconds and convert from degrees to radian: Angle*(PI/180) = Angle*0,0174532925 

      
    index2++; //this is hust the index for defining how often the values should be transmitted to serial
    if (index2 > 10) index2 = 0;
    
    readTime = (millis() - timeRead); //not really needed, just for logging
    
    } 
    
    if(index2 == 10){
      //Transmission to Serial (Values used in Processing):    
      for (int m=0;m<3;m++){ //ignore the for, needed later in developing...
        Serial.print(1, DEC); //ignore
        Serial.print(";");
      }
      for (int m=0;m<3;m++){//ignore the for, needed later in developing...
        Serial.print(yrpAngle, DEC);
        Serial.print(";");
      }
      for (int m=0;m<3;m++){//ignore the for, needed later in developing...
        Serial.print(yrpAverageAngleSpeed, DEC);
        Serial.print(";");
      }      
      Serial.print(lastRead, DEC);
      Serial.print(";");
      Serial.print(readTime, DEC);
      Serial.print(";");
      Serial.print(timeCalibration, DEC);    
      Serial.print("]");
    }  
}


//WMP functions:
//-------------------------------------------

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 wmpReceiveData(){
  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++){
    wmpdata[i]=Wire.receive();
  }

  yrp=(((wmpdata[3]>>2)<<8)+wmpdata[0]);        //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus

  slowYaw = wmpdata[3] & 2; // Read if in fast or slow mode


  // Handle the fast/slow bits of the Wii MotionPlus
  yawScale   = slowYaw   ? wmpSlowToDegreePerSec : wmpFastToDegreePerSec;

  yrpRead = yrp/yawScale; // make degrees out of readings

  yrpRead = yrpRead-yrp0; // calibration

}

int wmpCalibrateZeroes(){
  int timeCalibration = int(millis());
  int numberOfAverageings = 200;
  for (int k=0;k<numberOfAverageings;k++){
    wmpSendZero();
    Wire.requestFrom(0x52,6);
    for (int j=0;j<6;j++){
      wmpdata[j]=Wire.receive();
    }
    yrp0+=(((wmpdata[3]>>2)<<8)+wmpdata[0]);        //average readings for each zero

 }

  yrp0 = (yrp0/numberOfAverageings)/wmpSlowToDegreePerSec;

 
  timeCalibration = int(millis()) - timeCalibration;
  return timeCalibration;
}

looked at the code quickly:

if (timeRead - millis() >= sampleIntervall)

should that not be

if (millis() - timeRead >= sampleIntervall) // millis is bigger than timeRead ...


Is it possible that some rounding errors add up so you don't reach 0 in the end?

==> an int division looses info. - yrpRead = yrp/yawScale; - yrp0 = (yrp0/numberOfAverageings)/wmpSlowToDegreePerSec;

wow, serious mistake there, thanks a lot.

now its much better, because the sample times are now really what they should be.

however i discovered there is logical problem:

what i do is to calculate the angle from yawspeed over time.

however, when you roll by 90°, then make a movement by 90° around the pitch axis, then roll back by -90° the yaw angle is 90° as well, but the calculated angle will be 0°.

the reason is simple: there has been no yawspeed, so there is noch chance you could measure yaw.

somehow this has to be regarded as well, probably by some vector model oder so?

has anyone a good hint for reading into this?

thanks!

ok... now what i thinks what i have to do is basically this:

http://www.starlino.com/imu_guide.html