Diff: main.cpp

Revision:

0:940bc2db4c86

diff -r 000000000000 -r 940bc2db4c86 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 06 03:19:51 2020 +0000
@@ -0,0 +1,54 @@
+
+#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);            
+        }
+    }
+}
+ 
\ No newline at end of file