main.cpp

00001 #include "mbed.h"
00002 #include "myMPU9250.h"
00003 
00004 DigitalOut g_led1(LED1), g_led2(LED2), g_led3(LED3), g_led4(LED4);
00005 Serial g_pc(USBTX, USBRX);
00006 
00007 void blink_led(double sec=3.0, double interval = 0.2,
00008                bool led1=true, bool led2=true, bool led3=true, bool led4=true)
00009 {
00010 
00011     for(double t=0.0 ; t<sec ; t+=interval*2.0) {
00012         g_led1 = (int)led1;
00013         g_led2 = (int)led2;
00014         g_led3 = (int)led3;
00015         g_led4 = (int)led4;
00016         wait(interval);
00017         g_led1 = g_led2 = g_led3 = g_led4 = 0;
00018         wait(interval);
00019     }
00020 }
00021 
00022 int main()
00023 {
00024     g_pc.printf("START!\n");
00025     MyMPU9250 imu;
00026     if(imu.initialize()) {
00027         g_pc.printf("SUCCESS TO INITIALIZE!\n");
00028         blink_led(3.0, 0.2, true, false, false, true);
00029     } else {
00030         blink_led();
00031         return 0;
00032     }
00033 
00034     imu.setup();
00035 
00036     double x, y, z;
00037     for(int cnt=0;; cnt++) {
00038         if(cnt%2==0) {
00039             imu.updateAccelAllAxes();
00040             x = imu.accelX();
00041             y = imu.accelY();
00042             z = imu.accelZ();
00043             g_pc.printf("Accel: %f, %f, %f \n", x, y, z);
00044         } else {
00045             imu.updateGyroAllAxes();
00046             x = imu.gyroX();
00047             y = imu.gyroY();
00048             z = imu.gyroZ();
00049             g_pc.printf("Gyro: %f, %f, %f \n", x, y, z);            
00050         }
00051     }
00052 }