Diff: main.cpp

Revision:

0:cecf4940adf1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 25 15:28:55 2015 +0000
@@ -0,0 +1,52 @@
+#include "mbed.h"
+#include "myMPU9250.h"
+
+DigitalOut g_led1(LED1), g_led2(LED2), g_led3(LED3), g_led4(LED4);
+Serial g_pc(USBTX, USBRX);
+
+void blink_led(double sec=3.0, double interval = 0.2,
+               bool led1=true, bool led2=true, bool led3=true, bool led4=true)
+{
+
+    for(double t=0.0 ; t<sec ; t+=interval*2.0) {
+        g_led1 = (int)led1;
+        g_led2 = (int)led2;
+        g_led3 = (int)led3;
+        g_led4 = (int)led4;
+        wait(interval);
+        g_led1 = g_led2 = g_led3 = g_led4 = 0;
+        wait(interval);
+    }
+}
+
+int main()
+{
+    g_pc.printf("START!\n");
+    MyMPU9250 imu;
+    if(imu.initialize()) {
+        g_pc.printf("SUCCESS TO INITIALIZE!\n");
+        blink_led(3.0, 0.2, true, false, false, true);
+    } else {
+        blink_led();
+        return 0;
+    }
+
+    imu.setup();
+
+    double x, y, z;
+    for(int cnt=0;; cnt++) {
+        if(cnt%2==0) {
+            imu.updateAccelAllAxes();
+            x = imu.accelX();
+            y = imu.accelY();
+            z = imu.accelZ();
+            g_pc.printf("Accel: %f, %f, %f \n", x, y, z);
+        } else {
+            imu.updateGyroAllAxes();
+            x = imu.gyroX();
+            y = imu.gyroY();
+            z = imu.gyroZ();
+            g_pc.printf("Gyro: %f, %f, %f \n", x, y, z);            
+        }
+    }
+}