Wii Motion Plus at saturation ?? Need Help !

Using Arduino Sensors
#1

Hi everyone,

Here's the data I get from a wii motion plus alone :


(this is the roll curve, but it's the same for pitch and yaw)

I'm just moving the wmp by hand, so I hardly think I can reach the 2000°/sec limit like that. Is it possible that the gyro has drifted so much it can be saturated so easily ?

I'm suspecting something else because it turns out I never get positive and negative values from the wmp at the same time, it is always only positive, and the saturation happens when it reaches 0. At first, I thought I used unsigned variables but it turns out no, I've checked the whole code about 5 times already :grin:

Any ideas ?

#2

Could somebody try my code with a working/original wmp & nunchuk just to check is the problem isn't software related ? That would be awesome :wink:

Here is the arduino and processing code : http://www.woofiles.com/dl-300353-IuQvo6tw-code.rar

EDIT : After some further testing, the problem doesn't seem to come from a variable overflow, nor serial transmission, nor the processing application. I must really have a shitty sensor... damn

#3

Can you post the code in CODE tags, no-one really wants to have to unpack an archive...

#4

Could this condition be one of level shifting? where the ADC input needs to be 1/2 * Vref? and be capacitvely coupled to the WII output?

Doc

#5

@Doc : I don't think I have to deal with the ADC like that, I mean, if the wii motion plus was original, I would definitively not have to. Since it's a fake one maybe your fix might work. Is that what you meant ?
@Markt : Here it comes :

#include <Wire.h>
#include "structures.h"
#include "WiiSensors.h"
#include "MotorsController.h"
#include "Filter.h"

//#define TWI_FREQ_NUNCHUCK 400000L
#define TWI_FREQ_NUNCHUCK 100000L

void setup()
{
  Serial.begin(115200);
  Wire.begin();
    //i2c frequency, TWBR = WTF ?
  TWBR = ((16000000L / TWI_FREQ_NUNCHUCK) - 16) / 2;

  WiiSensor.Setup();
  WiiSensor.Power(true);
  delay(200);
  WiiSensor.Init();
  delay(100);
  WiiSensor.Calibration();  
}
void loop()
{
  gyroData gData;
  vector3f angles;
  WiiSensor.UpdateData();
  
  gData = WiiSensor.GetGyroData();
  angles = WiiSensor.GetEulerAngles();
 
  Serial.print(gData.roll);
  Serial.print(",");
  Serial.print(gData.pitch);
  Serial.print(",");
  Serial.print(gData.yaw);
  Serial.print(",");
  Serial.print(angles.x);
  Serial.print(",");
  Serial.print(angles.y);
  Serial.print(",");
  Serial.println(angles.z);
  delay(20);
}

WiiSensors.cpp :

#include "WiiSensors.h"
#include "Wire.h"

WiiSensors::WiiSensors()
{
  for(int i(0) ; i < 4 ; ++i)
  {
    gdata[i].roll = 0;
    gdata[i].pitch = 0;
    gdata[i].yaw = 0;
  }
  
  adata.x = 0;
  adata.y = 0;
  adata.z = 0;
  
  angles.x = 0;
  angles.y = 0;
  angles.z = 0;
  
  wmpDone = false;
  nckDone = false;
  
  currenttime = millis();
  lasttime = currenttime;
}

WiiSensors::~WiiSensors()
{
  
}

void WiiSensors::Setup()
{
  pinMode(12,OUTPUT);//Pin d'alim
}

void WiiSensors::Power(boolean setOn)
{
  if(setOn)
    digitalWrite(12, HIGH);
  else
    digitalWrite(12, LOW);
}

void WiiSensors::Init()
{
  // ************************************************************************************************************
  // I2C Wii Motion Plus + optional Nunchuk
  // ************************************************************************************************************
  // WMP addresse : 0xA6 (8bit) corresponding I2C adress   0x53 (7bit) (writing)
  // WMP addresse : 0xA4 (8bit) corresponding I2C adress   0x52 (7bit) (reading) les données sont en 0xA40008
  // ************************************************************************************************************ 
  delay(100);
  //Activation wii motion plus + nunchuk en pass trough mode
  
  
  Wire.beginTransmission(0x53);
  Wire.write(0xF0);
  Wire.write(0x55);
  Wire.endTransmission();
  
  delay (100);
  
  //adresse du port i2c en ecriture
  Wire.beginTransmission(0x53);
  //Addresse d'ecriture
  Wire.write(0xFE);
  //Valeur à écrire
  Wire.write(0x05);
  //Envoi des bytes
  Wire.endTransmission();
  
  
  //Normalement c'est initialisé
  this->IsActive();
}

void WiiSensors::Calibration()
{
  
  int timeCalibration = int(millis());
  int numberOfAverageings = 20;
  int realNumber = 0;
  
  rollZero = 0;
  pitchZero = 0;
  yawZero = 0;
  
    for (int k = 0 ; k < numberOfAverageings ; k++)
    {
      this->SendZeros();
      Wire.requestFrom(0x52,6);
      for(int i(0) ; i < 6 ; ++i)
        outbuf[i] = Wire.read();
        
      if((outbuf[5]&0x03) == 0x02) //WMP data
      {
        rollZero  += -(((outbuf[5]>>2)<<8) | outbuf[2] ); //range: +/- 8192
        pitchZero += -(((outbuf[4]>>2)<<8) | outbuf[1] );
        yawZero   +=  (((outbuf[3]>>2)<<8) | outbuf[0] );
        realNumber++;
      }
      delay(10);
  }
  
  rollZero /= realNumber;
  pitchZero /= realNumber;
  yawZero /= realNumber;
  Serial.println("real=");
  Serial.println(realNumber);
  Serial.println("avg = ");
  Serial.println(rollZero);
}

void WiiSensors::UpdateData()
{
  int cmpt = 0;
  this->ReadData();
  delay(3);
  //Tant que l'on n'a pas mis à jour les deux données
  while((wmpDone && nckDone) && cmpt < 20)
  {
    this->ReadData();
    cmpt++;
    delay(3);
  }
  //Filtrage
  
  currenttime = millis();
  angles.x += this->RK4Integrate(gdata[3].roll,  gdata[2].roll,  gdata[1].roll,  gdata[0].roll,  currenttime-lasttime);
  angles.y += this->RK4Integrate(gdata[3].pitch, gdata[2].pitch, gdata[1].pitch, gdata[0].pitch, currenttime-lasttime);
  angles.z += this->RK4Integrate(gdata[3].yaw,   gdata[2].yaw,   gdata[1].yaw,   gdata[0].yaw,   currenttime-lasttime);
  lasttime = currenttime;
  
  wmpDone = false;
  nckDone = false;
}

void WiiSensors::ReadData()
{  
  this->SendZeros();
  
  Wire.requestFrom(0x52,6);
  for(int i(0) ; i < 6 ; ++i)
      outbuf[i] = Wire.read();
      
  if((outbuf[5]&0x03) == 0x02) //WMP data
  {
    for(int i(0) ; i < 3 ; ++i)
    {
      gdata[i].roll = gdata[i+1].roll;
      gdata[i].pitch = gdata[i+1].pitch;
      gdata[i].yaw = gdata[i+1].yaw;
    }
    
    gdata[3].roll =  -(((outbuf[5]>>2)<<8) | outbuf[2] ); //range: +/- 8192
    gdata[3].pitch = -(((outbuf[4]>>2)<<8) | outbuf[1] );
    gdata[3].yaw =   (((outbuf[3]>>2)<<8) | outbuf[0] );
    
    //Zero compensation
    gdata[3].roll -= rollZero;
    gdata[3].pitch -= pitchZero;
    gdata[3].yaw -= yawZero;
    
    // Check if slow bit is set and normalize to fast mode range
    gdata[3].roll  = (outbuf[3]&0x01)     ? gdata[3].roll/5  : gdata[3].roll;  //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification 
    gdata[3].pitch = (outbuf[4]&0x02)>>1  ? gdata[3].pitch/5 : gdata[3].pitch; //we detect here the slow of fast mode WMP gyros values (see wiibrew for more details)
    gdata[3].yaw   = (outbuf[3]&0x02)>>1  ? gdata[3].yaw/5   : gdata[3].yaw;
    
    gdata[3].roll /= 40;
    gdata[3].pitch /= 40;
    gdata[3].yaw /= 40;
    
    wmpDone = true;
    
  }
  else if ((outbuf[5]&0x03) == 0x00) // Nunchuk Data
  { 
    //Peut être à changer les signes ici
    adata.x = - ((outbuf[3]<<2) | ((outbuf[5]>>4)&0x02));
    adata.y =   ((outbuf[2]<<2) | ((outbuf[5]>>3)&0x02));
    adata.z =   ((outbuf[4]>>1)<<3) | ((outbuf[5]>>5)&0x06);
    
    nckDone = true;
  }
}
byte WiiSensors::IsActive()
{
  
  delay (100);
  //adresse du port i2c en lecture
  Wire.beginTransmission(0x52);
  //On set l'adresse de lecture
  Wire.write(0xFA);
  Wire.endTransmission();
  delay (100);
  //On lit 6 bits depuis l'adresse 0x52
  Wire.requestFrom (0x52,6);
  byte outbuf[6];
  outbuf[0] = Wire.read();Serial.print(outbuf[0],HEX);Serial.print(" ");
  outbuf[1] = Wire.read();Serial.print(outbuf[1],HEX);Serial.print(" ");
  outbuf[2] = Wire.read();Serial.print(outbuf[2],HEX);Serial.print(" ");
  outbuf[3] = Wire.read();Serial.print(outbuf[3],HEX);Serial.print(" ");
  outbuf[4] = Wire.read();Serial.print(outbuf[4],HEX);Serial.print(" ");
  outbuf[5] = Wire.read();Serial.print(outbuf[5],HEX);Serial.print(" ");
  Serial.print ("\r\n");
  
  byte xID= outbuf[0] + outbuf[1] + outbuf[2] + outbuf[3] + outbuf[4] + outbuf[5];
  Serial.print("Ext control xID = 0x");
  Serial.print(xID,HEX);
  if (xID == 0xCB) { Serial.print (" WM+ connected, not activated"); }
  if (xID == 0xCE) { Serial.print (" WM+ connected & activated"); }
  if (xID == 0x00) { Serial.print (" WM+ not connected"); }
  Serial.print ("\r\n");
  
  delay (100);
  //On rétabli l'adresse en lecture des données gyr+acc
  Wire.beginTransmission(0x52);
  Wire.write(0x08);
  Wire.endTransmission();
  delay(100);
  
  //On essaye de lire des données du nunchuk
  nckDone = false;
  int cmpt = 0;
  while(cmpt < 20 && nckDone == false)
  {
   this->ReadData();
   delay(4);
   Serial.print("attempt ");
   Serial.println(cmpt);
   cmpt++;
  }
  
  if(nckDone)
    Serial.println("Nunchuck connected");
  else
    Serial.println("Nunchuck not detected");
  
  return xID;
}
gyroData WiiSensors::GetGyroData()
{
  return gdata[3];
}
vector3 WiiSensors::GetAccData()
{
  return adata;
}

vector3f WiiSensors::GetEulerAngles()
{
  return angles;
}
  
void WiiSensors::SendZeros()
{
  Wire.beginTransmission(0x52);    //at address 0x52
  Wire.write(byte(0x00));       //send zero to ask for data
  Wire.endTransmission();
  delayMicroseconds(200);
}

float WiiSensors::RK4Integrate(int data4, int data3, int data2, int data1, int deltaTmillis)
{
  float area=0;
  area =((data4+2*data3+2*data2+data1)/6)*deltaTmillis/1000.f;
  return area;
}

//Instantiate WiiSensor
WiiSensors WiiSensor = WiiSensors();
#6

So which roll variable are you plotting? The raw roll data from the gyro or a processed version?

#7

I'm plotting the raw roll curve. The problem is also happening for raw pitch and yaw.

#8

Allright now I'm sure there is something wrong with the sensor, I've switched the supply power of the wmp to 3.3, no difference, so I've removed the calibration of the gyro, the average value is around 8100, which makes perfect sense (half of the full-scale), the lowest value I get and for which the saturation happens is of course... 0. I've somehow managed to find one of the rare really crappy Wii Motion Plus copy.
It would have been great to confirm all of that with an other WMP but nevermind.

#9