Wii motion plus and webbotlib

[totalis]

I have recently been playing with the gyro fromm the wii motion plus, It is quite a nice device that has been interfaced with many times using the arduino library. I have successfully compiled the ardduino version and that works well. i am now attempting to emulate that using webbotlib.

The code i have written is as close to a 'copy' of the arduino version as i can muster, however, it is not returning any values, merely 0 for each value. It seems that the recieve function is failing, but i dont know enough about i2c to understand why.

Code: [Select]

#define UART_TX_BUFFER_SIZE 20 //These are defined to reduce the delay
//#define UART_RX_BUFFER_SIZE 40 //and data loss in the send and recieve buffers

#include "sys/atmega328P.h" //These are the includes required by the webbotlib library.
#include "uart.h"
#include "rprintf.h"
#include "i2cBus.h"

uint8_t data[6]; // data container for Calibrate data
int yaw, pitch, roll;  //three axes
int yaw0, pitch0, roll0;  //calibration zeroes


GENERIC_I2C_DEVICE WMDevOff = MAKE_GENERIC_I2C_DEVICE(0x53);
GENERIC_I2C_DEVICE WMDevOn  = MAKE_GENERIC_I2C_DEVICE(0x52);
static I2C_DEVICE_LIST PROGMEM i2c_list[] = {&WMDevOff.i2cInfo,&WMDevOn.i2cInfo};
I2C_HARDWARE_BUS i2c = MAKE_I2C_HARDWARE_BUS(i2c_list);

void Calibrate(){
for (int i=0;i<10;i++)
{

i2cMasterSend(&WMDevOn.i2cInfo, 1, 0x00);
boolean Recieve = i2cMasterReceive(&WMDevOn.i2cInfo, 6, data);
if (Recieve == FALSE) rprintfStr("Cal Fail");

yaw0+=(((data[3]>>2)<<8)+data[0])/10;        //average 10 readings for each zero
            pitch0+=(((data[4]>>2)<<8)+data[1])/10;
            roll0+=(((data[5]>>2)<<8)+data[2])/10;
}
rprintfStr("Yaw0: ");
rprintfNum(10, 4, TRUE, ' ', yaw0);
rprintfStr("Pitch0: ");
rprintfNum(10, 4, TRUE, ' ', pitch0);
rprintfStr("Roll0: ");
rprintfNum(10, 4, TRUE, ' ', roll0);
}

void receiveData(){
i2cMasterSend(&WMDevOn.i2cInfo, 1, 0x00);
boolean Recieve = i2cMasterReceive(&WMDevOn.i2cInfo, 6, data);
if (Recieve == FALSE) rprintfStr("Fail");
yaw=((data[3]>>2)<<8)+data[0]-yaw0;        //see http://wiibrew.org/wiki/Wiimote/Extension_Controllers#Wii_Motion_Plus
pitch=((data[4]>>2)<<8)+data[1]-pitch0;    //for info on what each byte represents
roll=((data[5]>>2)<<8)+data[2]-roll0;      
}

void appInitHardware(void)
{
uartInit(UART0, (BAUD_RATE)115200);
rprintfInit(&uart0SendByte);
i2cBusInit(&i2c);
i2cBusSetBitRate(&i2c, 400);
}

TICK_COUNT appInitSoftware(TICK_COUNT loopStart)//This is the main loop that provides timing by delaying by 1 cycle
{
rprintfStr("WM Test");
i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xFE, 0x04);

i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xF0, 0x55);
i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xFB, 0x00); //Turns the WM+ on
Calibrate();
    return 10000;
}

TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart)
{
receiveData();
rprintfStr("\nYaw: ");
rprintfNum(10, 4, TRUE, ' ', yaw);
rprintfStr("Pitch: ");
rprintfNum(10, 4, TRUE, ' ', pitch);
rprintfStr("Roll: ");
rprintfNum(10, 4, TRUE, ' ', roll);

return 100000;
}
any help is appreciated, The original arduino version is here: https://github.com/ril3y/Make-Projects/blob/master/Wii%20Motion%20Plus/Wii_Motion_Plus_DataReader/Wii_Motion_Plus_DataReader.pde

[Webbot]

The Arduino Wire functions use a 7 bit address whereas WebbotLib uses an 8 bit address.  This is a common 'issue' as some manufacturers data sheets use 7 bit whilst others use 8 bit - doesn't seem to be a 'standard'

So what you can try doing is multiplying the I2C addresses by 2 :-
change 0x52 to 0xA4
change 0x53 to 0xA6


See if that works.

[totalis]

well that explains alot about the other source code that i have found, i will try that soon. Thanks for the quick reply

also @Webbot: Is this device something you would support in you library if i wrote the code?

T

[Webbot]

Yes - but I would want to change the code slightly.....not coz your code is wrong but I would want to make it into a standard 'gyro' object and hence make it return values in degrees/second rather than just 'raw' values.

Once you've got it working, I'll do a short tutorial on how to do that so you can see what's involved and the steps to move from 'your' code to 'WebbotLib friendly' code.

Incidentally: I'm not sure the calibration code looks correct (it isn't is the Arduino sketch either). It looks like the intention is to take 10 readings and then divide the sum by 10 to give an average. But the current code divides by 10 on each iteration around the loop.

Also: looking at http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Wii_Motion_Plus it appears that you should be testing the 3 bits to detect for slow vs fast mode as the data bytes are then treated differently.

[waltr]

Phillips does have a standard for I2C addressing (the standard is available on Phillip's web site). It is 7 bits but left justified. The lsb is the R/W bit and this is where everyone gets confused including manufacture's data sheets.

So for the device mentioned, if the 7-bit address is 0x52  which is then shifted left one bit (multiply by 2) to become 0xA4 in the I2C control byte. The lsb is '0' for a write or '1' for a read. Technically the lsb is not part of the address in the 8bit control byte it is the R/W bit which is set or cleared depending on the operation to be performed.
If a Write operation is required then send 0xA4, lsb = '0'.
If a Read operation is required then send 0xA5, lsb = '1'.

[Webbot]

Yep WebbotLib supports the 'Phillips' standard, Arduino Wire doesn't (it uses 7 bit right justified). But the main issue I referred to is that other hardware manufacturers seem to randomly adopt the left or right justified format so can be confusing.

In terms of setting the lsb to 0/1 for write/read then WebbotLib does this for you automatically. Hence all I2C addresses specified in WebbotLib when creating device should be 'even' numbers.

[totalis]

Yes - but I would want to change the code slightly.....not coz your code is wrong but I would want to make it into a standard 'gyro' object and hence make it return values in degrees/second rather than just 'raw' values.

Once you've got it working, I'll do a short tutorial on how to do that so you can see what's involved and the steps to move from 'your' code to 'WebbotLib friendly' code.

I completely agree, this is only the first of many tests and it will be a while before i have a set of code im happy with. I am both using this as a learning exercise and a way of adding devices to webbotlib.

Incidentally: I'm not sure the calibration code looks correct (it isn't is the Arduino sketch either). It looks like the intention is to take 10 readings and then divide the sum by 10 to give an average. But the current code divides by 10 on each iteration around the loop.

Also: looking at http://wiibrew.org/wiki/Wiimote/Extension_Controllers/Wii_Motion_Plus it appears that you should be testing the 3 bits to detect for slow vs fast mode as the data bytes are then treated differently.

Yeah i had wondered about that, i have yet to investogate as i wanted to get some functional code first then mess with it later, Although im not sure about the slow vs fast mode thing, the arduino code doesnt even touch on it...

Im testing now
(all addresses are doubled, including the register addresses?)

T

[Webbot]

(all addresses are doubled, including the register addresses?)

Nope only the I2C addresses on these lines:-

Code: [Select]

GENERIC_I2C_DEVICE WMDevOff = MAKE_GENERIC_I2C_DEVICE(0x53);
GENERIC_I2C_DEVICE WMDevOn  = MAKE_GENERIC_I2C_DEVICE(0x52);
become

Code: [Select]

GENERIC_I2C_DEVICE WMDevOff = MAKE_GENERIC_I2C_DEVICE(0xA6);
GENERIC_I2C_DEVICE WMDevOn  = MAKE_GENERIC_I2C_DEVICE(0xA4);

Everything else stays the same as before

[totalis]

Cool, i have tried again with the new registers and the read is not failing, but the values are not updating each loop, I think it is something to do with the data variable and the way the Arduino code treated it, but atleast the thing seems to be working now

[totalis]

Forgot: Thanks for the stupidly fast response Cheers

[totalis]

@Webbot, Is there anything special that happens to the data when it is read (8-bit mode etc...)? i am getting some weird responses from the gyro, when the gyro first sends data the values are reasonable but they very quickly saturate the uint8_t.

The calibrate code is weird and it does seem that the gyro has 2 modes to deal with (on the todo list) but i believe the data is not updating between reads


  130  130    0    0    0    0
   80  185  123  131  126  126
  255  125  136  118  240  118
  255  255  255  125  136  118
  255  255  255  125  136  118
  255  255  255  125  136  118
  255  255  255  125  136  118
  255  255  255  125  136  118
  255  255  255  125  136  118
  255  255  255  125  136  118

Above it the values of data[] after each calibration loop.

[totalis]

I have found that if i add a delay into the recieving code, the values all change to


  126  126    0    0    0    0
   50  125  247  135  178   49
   50  125  247  135  178   49
   50  125  247  135  178   49
   50  125  247  135  178   49
   50  125  247  135  178   49
   50  125  247  135  178   49
   50  125  247  135  178   49
   50  125  247  135  178   49
   50  125  247  135  178   49



This isnt fair...

[totalis]

Backtracking to find a problem has revealed that the code to initialise the WM+ has failed, it says that this line of code:

Code: [Select]

i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xFE, 0x04);//Turns the WM+ on
failed, any ideas what im doing wrong?

[Webbot]

Could try changing the order of stuff.
your code has:

Code: [Select]

i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xFE, 0x04);

i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xF0, 0x55);
i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xFB, 0x00); //Turns the WM+ on


The last 2 lines aren't used in the Arduino code - so try getting rid of them, or try changing the order:-

Code: [Select]


i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xF0, 0x55);
i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xFB, 0x00); //Turns the WM+ on

i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xFE, 0x04);

Reason being that after writing 0x04 to register 0xFE the devicve changes I2C address (to WMDevOn) - as per the html link given previously.

[totalis]

Sorry webbot, those lines were just used to test and are not included (anymore)

The above problem seems to be caused by the gyro already active when i retry to address is(Reset the MCU). All is well when i capture the gyro data for the first time, its just subsequent times that cause issues, this may be solved by de activating the gyro each read, or possibly by forgoing the re-initialisation. I will investigate tomorrow.

amazing how, in one day, it is possibly (with the right tools) to go from a non functioning mystic set of chips and wires, To a (mostly) functioning gyro!!!!!!

[Webbot]

The URL I linked to earlier gives you a way to read some signature bytes to see if the gyro is the default device or not. You could check this in your initialisation code - and you could also use it to test if the device is plugged in and working or not.

[totalis]

The URL I linked to earlier gives you a way to read some signature bytes to see if the gyro is the default device or not. You could check this in your initialisation code - and you could also use it to test if the device is plugged in and working or not.

A very good idea, im on it

I have tried again today and found that the gyro will work (for a little while) if it has been disconnected from the power, then reconnected, and the MCU then reset, It is very weird though how the arduino code works fine but my webbotlib equivalent will not.

Just to verify:

Arduino:

Code: [Select]

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
}
==

Webbotlib:

Code: [Select]

i2cMasterWriteRegister(&WMDevOff.i2cInfo, 0xFE, 0x04);//Turns the WM+ on
And

Arduino:

Code: [Select]

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

    wmpSendZero();
    Wire.requestFrom(0x52,6);
    for (int i=0;i<6;i++){
      data[i]=Wire.receive();
    }
==

Webbotlib:

Code: [Select]

i2cMasterTransfer(&WMDevOn.i2cInfo, 1, 0x00, 6, data);

[Webbot]

Webbotlib:

Code: [Select]

i2cMasterTransfer(&WMDevOn.i2cInfo, 1, 0x00, 6, data);

I would use the i2cMasterReadRegisters function to read 6 bytes from address 0.

[totalis]

like this ?

Code: [Select]

i2cMasterReadRegisters(&WMDevOn.i2cInfo, 0, 6, data);

[totalis]

The device responds like it should!!!!!!!!!!!!!!!!!!!!! Wow im glad you helped, i would have been scratching my head for days...

so, i have to ask, What is the difference between transfer and readregisters?

Thanks Webbot!

T

[Webbot]

so, i have to ask, What is the difference between transfer and readregisters?

They are similar. 'transfer' is used to send some data and then receive some data. 'readregisters' actually calls 'transfer' but is just a more user friendly way of using it to avoid errors like yours ;-)

Your 'transfer' code would have worked if it had been like this:-

Code: [Select]

uint8_t reg = 0;
i2cMasterTransfer(&WMDevOn.i2cInfo, 1, &reg, 6, data);
Note how the 3rd parameter should be the address of the data that you want to send. Your code just had '0x00' so it will write out the byte stored at location '0x00' in RAM - anyones guess what that byte would be !! Whereas my code above uses it to reference the 'reg' variable which stores the value of 0 that you want to send.

Anyway - glad its going