Diff: Sensors.h

Revision:

0:0bbf2f16da9c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors.h	Fri Nov 18 18:23:33 2011 +0000
@@ -0,0 +1,100 @@
+
+//In here be functions related to Sensors
+
+//
+void setupaccel() {
+    i2c.start();
+    i2c.write(ACCADDRESS << 1 | 0);
+    i2c.write(7);
+    i2c.write(1);
+    i2c.stop();
+}
+
+//
+void getaccel(signed char returnbuff[3]) {
+    i2c.start();
+    i2c.write(ACCADDRESS << 1 | 0);
+    i2c.write(0);
+    i2c.stop();
+
+    //Read the axis data
+    i2c.read(ACCADDRESS << 1 | 1, (char*)returnbuff, 3);
+
+    for (int i = 0; i < 3; i++) {
+        returnbuff[i] = returnbuff[i] << 2;
+        returnbuff[i] /= 4;
+    }
+
+}
+
+void calibgyro() {
+
+    //activating the wiimotion +
+    i2c.start();
+    //device address | write
+    i2c.write(0x53 << 1 | 0);
+    //register address
+    i2c.write(0xfe);
+    //data
+    i2c.write(0x04);
+    i2c.stop();
+
+    for (int i = 0; i < 100; i++) {
+        //sending a 0x00 to flag that we want data
+        i2c.start();
+        //device address | write (note new address)
+        i2c.write(0x52 << 1 | 0);
+        //send 0x00
+        i2c.write(0x00);
+        i2c.stop();
+
+        //reading the data
+        char wmpdata[6];
+        i2c.read(0x52 << 1 | 1, wmpdata, 6);
+        //pc.printf("%x %x %x %x %x %x\r\n",  wmpdata[0], wmpdata[1], wmpdata[2], wmpdata[3], wmpdata[4], wmpdata[5]);
+
+        gyrcalib[0] += (((wmpdata[3] >> 2) << 8) + wmpdata[0]) / 100;
+        gyrcalib[1] += (((wmpdata[4] >> 2) << 8) + wmpdata[1]) / 100;
+        gyrcalib[2] += (((wmpdata[5] >> 2) << 8) + wmpdata[2]) / 100;
+
+        wait(0.05);
+
+    }
+
+}
+
+void getgyros(float gyrodata[3]) {
+    //sending a 0x00 to flag that we want data
+    i2c.start();
+    //device address | write (note new address)
+    i2c.write(0x52 << 1 | 0);
+    //send 0x00
+    i2c.write(0x00);
+    i2c.stop();
+
+    //reading the data
+    char wmpdata[6];
+    i2c.read(0x52 << 1 | 1, wmpdata, 6);
+    
+    //detect if we ever went into fast mode
+    bool fastdiscard = !(wmpdata[3] & 0x02 && wmpdata[4] & 0x02 && wmpdata[3] & 0x01);
+    
+    if (fastdiscard)
+        myled = 1;
+    else {
+        gyrodata[0] = RPSPUNIT * -(((wmpdata[3] >> 2) << 8) + wmpdata[0] - gyrcalib[0]);
+        gyrodata[1] = RPSPUNIT * (((wmpdata[4] >> 2) << 8) + wmpdata[1] - gyrcalib[1]);
+        gyrodata[2] = RPSPUNIT * (((wmpdata[5] >> 2) << 8) + wmpdata[2] - gyrcalib[2]);
+    }
+
+    //pc.printf("yaw: %f, pitch: %f, roll: %f\r\n", gyrodata[0], gyrodata[1], gyrodata[2]);
+
+    //wait(0.05);
+
+}
+
+#ifdef ALTSENSOR
+float getaltsensor() {
+    return altsensor * 
+}
+#endif
\ No newline at end of file