@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();