WiiMP.c

00001 #include "WiiMP.h"
00002 
00003 
00004 
00005 WiiMP::WiiMP(PinName data, PinName clk):_i2c(data, clk) {
00006     _i2c.frequency(100000); // Some report success at 400000, but 100000 should be guaranteed
00007     Error = true;
00008     
00009     //initialize whatever is at 0xA4 (if anything) -- will turn off MP+ if it's already on
00010     unsigned char cmd[] = {0xF0, 0x55};
00011     _i2c.write(WIIEXT_ADDR, (const char*)cmd, sizeof(cmd));
00012     wait_ms(I2C_READ_DELAY);
00013     
00014     // Instruct WiiMP to change address from 0xA6 to 0xA4 and initialize
00015     cmd[0] = 0xfe; cmd[1]= 0x04;
00016     if (_i2c.write(WMP_ADDR, (const char*)cmd, sizeof(cmd)) == I2C_ACK) {
00017         Error = false;
00018     }
00019     
00020 }
00021 
00022 bool WiiMP::Read(int* Yaw,int* Roll,int* Pitch) {
00023 
00024     char readBuf[WIIMP_READLEN];
00025     
00026     if (Error) {
00027         return false;
00028     }
00029     
00030     const unsigned char cmd[] = {0x00};
00031     if (_i2c.write(WIIEXT_ADDR, (const char*)cmd, sizeof(cmd)) == I2C_ACK) {
00032         wait_ms(I2C_READ_DELAY);
00033         if (_i2c.read(WIIEXT_ADDR, readBuf, sizeof(readBuf)) == I2C_ACK) {
00034             //init values
00035             *Yaw = 0; *Roll = 0; *Pitch = 0;
00036             
00037             *Yaw   = ((readBuf[YawH] >> 2) << 8) + readBuf[YawL];
00038             *Roll  = ((readBuf[RollH] >> 2) << 8) + readBuf[RollL];
00039             *Pitch = ((readBuf[PitchH] >> 2) << 8) + readBuf[PitchL];
00040                         
00041         }
00042         else
00043         {
00044             return false;
00045         }
00046     } else {
00047         return false;
00048     }
00049     return true;
00050 }