Diff: KXP84.cpp

Revision:

0:af16997a2d22

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/KXP84.cpp	Wed Jun 20 22:38:48 2012 +0000
@@ -0,0 +1,94 @@
+#include "mbed.h"
+#include "KXP84.h"
+
+void KXP84::init() {
+    char cmd[1];
+    //reset=0;
+    i2c.frequency(400000);
+//CTRL_REGB
+    i2c.start();
+    wait_us(1);
+    i2c.write(0x30);
+    wait_us(1);
+    *cmd=0x06;
+    i2c.write(0x0B,cmd,1);
+    wait_us(1);
+    i2c.stop();
+//CTRL_REGC
+    i2c.start();
+    wait_us(1);
+    i2c.write(0x30);
+    wait_us(1);
+    *cmd=0x00;
+    i2c.write(0x0A,cmd,1);
+    wait_us(1);
+    i2c.stop();
+//FF_INT
+    i2c.start();
+    wait_us(1);
+    i2c.write(0x30);
+    wait_us(1);
+    *cmd=0x14;
+    i2c.write(0x06,cmd,1);
+    wait_us(1);
+    i2c.stop();
+//FF_DELAY
+    i2c.start();
+    wait_us(1);
+    i2c.write(0x30);
+    wait_us(1);
+    *cmd=0x14;
+    i2c.write(0x07,cmd,1);
+    wait_us(1);
+    i2c.stop();
+//MOT_INT
+    i2c.start();
+    wait_us(1);
+    i2c.write(0x30);
+    wait_us(1);
+    *cmd=0x4D;
+    i2c.write(0x08,cmd,1);
+    wait_us(1);
+    i2c.stop();
+//MOT_DELAYC
+    i2c.start();
+    wait_us(1);
+    i2c.write(0x30);
+    wait_us(1);
+    *cmd=0x14;
+    i2c.write(0x09,cmd,1);
+    wait_us(1);
+    i2c.stop();
+}
+
+void KXP84::get(int *x, int *y, int *z) {
+    char data[6];
+    int dx,dy,dz;
+    i2c.start();
+    wait_us(1);
+    i2c.write(0x30);
+    wait_us(1);
+    i2c.write(0x00);
+    wait_us(1);
+    i2c.read(0x30,data,6);
+    wait_us(1);
+    i2c.stop();
+
+    dx=data[0]<<4 | data[1]>>4;
+    dy=data[2]<<4 | data[3]>>4;
+    dz=data[4]<<4 | data[5]>>4;
+
+    //if (dx>2048)
+    //    *x=dx-2048;
+    //else
+    *x=(int)(( (dx - 2048) * 1.24)+0.5);
+
+    //if (dy>2048)
+    //    *y=dy-2048;
+    //else
+    *y=(int)(( (dy - 2048) * 1.24)+0.5);
+    //if (dz>2048)
+    //    *z=dz-2048;
+    //else
+    *z=(int)(( (dz - 2048) * 1.24)+0.5);
+}
\ No newline at end of file