Diff: WiiMP.c

Revision:

0:506079387c48

Child:

1:727906cb5051

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/WiiMP.c	Mon Mar 14 22:39:59 2011 +0000
@@ -0,0 +1,54 @@
+#include "WiiMP.h"
+
+
+
+WiiMP::WiiMP(PinName data, PinName clk):_i2c(data, clk) {
+    _i2c.frequency(100000); // Some report success at 400000, but 100000 should be guaranteed
+    Error = true;
+    
+    //initialize whatever is at 0xA4 (if anything) -- will turn off MP+ if it's already on
+    unsigned char cmd[] = {0xF0, 0x55};
+    _i2c.write(WIIEXT_ADDR, (const char*)cmd, sizeof(cmd));
+    wait_ms(I2C_READ_DELAY);
+    
+    // Instruct WiiMP to change address from 0xA6 to 0xA4 and initialize
+    cmd[0] = 0xfe; cmd[1]= 0x04;
+    if (_i2c.write(WMP_ADDR, (const char*)cmd, sizeof(cmd)) == I2C_ACK) {
+        Error = false;
+    }
+    
+}
+
+bool WiiMP::Read(int* Yaw,int* Roll,int* Pitch) {
+
+    int i;
+    char readBuf[WIIMP_READLEN];
+    
+    if (Error) {
+        return false;
+    }
+    
+    const unsigned char cmd[] = {0x00};
+    if (_i2c.write(WIIEXT_ADDR, (const char*)cmd, sizeof(cmd)) == I2C_ACK) {
+        wait_ms(I2C_READ_DELAY);
+        if (_i2c.read(WIIEXT_ADDR, readBuf, sizeof(readBuf)) == I2C_ACK) {
+            //init values
+            *Yaw = 0; *Roll = 0; *Pitch = 0;
+   
+            for (i = 0; i < WIIMP_READLEN; ++i) {
+                readBuf[i] = (readBuf[i] ^ 0x17) + 0x17;
+            }
+            *Yaw   = ((readBuf[YawH] >> 2) << 8) + readBuf[YawL];
+            *Roll  = ((readBuf[RollH] >> 2) << 8) + readBuf[RollL];
+            *Pitch = ((readBuf[PitchH] >> 2) << 8) + readBuf[PitchL];
+                        
+        }
+        else
+        {
+            return false;
+        }
+    } else {
+        return false;
+    }
+    return true;
+}