Diff: main.cpp

Revision:

0:2b2172edbc90

Child:

1:cc13fddeedc0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 05 18:08:44 2011 +0000
@@ -0,0 +1,53 @@
+#include "mbed.h"
+
+DigitalOut myled(LED1);
+AnalogIn ainAccX(p20);
+AnalogIn ainAccY(p19);
+AnalogIn ainAccZ(p18);
+Serial pc(USBTX, USBRX);
+Ticker ticker;
+
+int calibacc3ax[3] = {0,0,0};
+float grav;
+
+float a = 0;
+double v = 0;
+double x = 0;
+
+unsigned int normalacc() {
+    int x = ainAccX.read_u16() - calibacc3ax[0];
+    int y = ainAccY.read_u16() - calibacc3ax[1];
+    int z = ainAccZ.read_u16() - calibacc3ax[2];
+    return (sqrt(float(x*x + y*y + z*z)));
+}
+
+void tick(){
+    a = normalacc() * 3.3 * 32.27 * 0.001 * (1.0 / 0x10000);
+    v += a;
+    x += v * 0.001;
+}
+
+int main() {
+    pc.baud(115200);
+    
+    for (int i = 0; i < 10000; i++) {
+        calibacc3ax[0] += ainAccX.read_u16();
+        calibacc3ax[1] += ainAccY.read_u16();
+        calibacc3ax[2] += ainAccZ.read_u16();
+        
+        wait_us(1000);
+    }
+
+    calibacc3ax[0] /= 10000;
+    calibacc3ax[1] /= 10000;
+    calibacc3ax[2] /= 10000;
+    
+    grav = //TODO: Make calibration for both +z and -z
+    
+    ticker.attach_us(&tick, 1000);
+    
+    while (1) {
+        pc.printf("%f %f %f\r\n", a * 1000, v, x);
+        wait(0.2);
+    }
+}