main.cpp

00001 //#include <events/mbed_events.h>
00002 #include <mbed.h>
00003 #include "MPU6050.h"
00004 #include <string>
00005 #include <stdio.h>
00006 
00007 
00008 
00009 DigitalOut led(p25, 1);
00010 MPU6050 mpu6050;
00011 Serial pc(USBTX,USBRX);
00012 
00013 int main()
00014 {
00015     pc.baud(115200);
00016     pc.printf("START_1\n");
00017     mpu6050.calibrate(accelBias, gyroBias);
00018     mpu6050.initialize();
00019     pc.printf("START_2\n");
00020     
00021    
00022  
00023  /////////////////////////////////////////////////////////SERIAL////////////////////////////////////////////
00024     while(0){
00025         led = 0;
00026         wait(1);
00027    if(pc.readable()){
00028        if(pc.getc()=='.'){
00029             pc.putc(pc.getc());
00030             led=1;
00031           wait(1);
00032           }
00033             }
00034   
00035         }
00036        
00037  /////////////////////////////////////////////////////////SERIAL/////////////////////////////////////////////
00038  
00039  /////////////////////////////////////////////////////////MPU6050////////////////////////////////////////////
00040  
00041     while(0){
00042          pc.printf("i am %d\n",mpu6050.getDeviceID());
00043          wait(1);
00044         }
00045        
00046  /////////////////////////////////////////////////////////MPU6050/////////////////////////////////////////////
00047  
00048  
00049  
00050  //////////////////////////////////////////////////////////////GET_DATA////////////////////////////////////////////
00051     while(1){
00052  
00053     mpu6050.readAccelData(accelData);
00054     mpu6050.getAres();
00055     ax = accelData[0]*aRes  - accelBias[0];
00056     ay = accelData[1]*aRes  - accelBias[1];
00057     az = accelData[2]*aRes  - accelBias[2];
00058     float accelValues[3] = {ax,ay,az};
00059     mpu6050.readGyroData(gyroData);
00060     mpu6050.getGres();
00061     gx = gyroData[0]*gRes - gyroBias[0];
00062     gy = gyroData[1]*gRes - gyroBias[1];
00063     gz = gyroData[2]*gRes - gyroBias[2];
00064     float gyroValues[3] = {gx,gy,gz};
00065         
00066     if(pc.readable())
00067     {
00068         if(pc.getc()=='.'){
00069             
00070             pc.printf("%f, ",gx);
00071             pc.printf("%f, ",gy);
00072             pc.printf("%f",gz);
00073            
00074         }
00075 
00076     }
00077     
00078     }
00079 }