main.cpp

00001 #include "MPU6050.h"
00002 #include "mbed.h"
00003 #include "SDFileSystem.h"
00004 
00005 SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd"); // MOSI, MISO, SCK, CS
00006 
00007 Serial pc(USBTX, USBRX);
00008 
00009 FILE *Data;   // Creates name of file
00010 char buffer[5000000];  // Buffer size (# of characters) for SD card
00011 
00012 MPU6050 mpu6050;
00013 Timer t;
00014 
00015 DigitalIn button(PTB9);
00016 
00017 float accx,accy,laccx=0,laccy=0;
00018 float vx,vy,lvx=0,lvy=0;
00019 float x,xx,y,yy;
00020 double dt, timer;
00021 
00022 bool closed = 1;    // Flag for sd card, to open and close with switch
00023 
00024 
00025 //================================================================================
00026 //=============               Sensor Initialization                  =============
00027 //================================================================================
00028 
00029 void gyro_init(){
00030     i2c.frequency(400000);
00031     
00032     uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
00033     pc.printf("I AM 0x%x\n\r", whoami);
00034     pc.printf("I SHOULD BE 0x68\n\r");
00035 
00036     if (whoami == 0x68){
00037         pc.printf("MPU6050 is online...");
00038         wait(1);
00039 
00040         mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
00041         if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
00042             mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
00043             mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
00044             mpu6050.initMPU6050();
00045             pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00046 
00047             pc.printf("\nMPU6050 passed self test... Initializing");
00048             wait(2); //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00049         } else pc.printf("\nDevice did not the pass self-test!\n\r");
00050     } else {
00051         pc.printf("Could not connect to MPU6050: \n\r");
00052         pc.printf("%#x \n",  whoami);
00053         while(1) ; // Loop forever if communication doesn't happen
00054     }
00055 }
00056 
00057 
00058 //================================================================================
00059 //=============                     Get Inputs                       =============
00060 //================================================================================
00061 
00062 void get_Rates(){
00063     if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
00064         mpu6050.readAccelData(accelCount);  // Read the x/y/z adc accel values
00065         mpu6050.getAres();
00066         accy = -(accelCount[0]*aRes - accelBias[0]); // g val
00067         accx = accelCount[1]*aRes - accelBias[1];   
00068         
00069         accx /= 9.81;
00070         accy /= 9.81;
00071     }
00072 }
00073 
00074 
00075 //================================================================================
00076 //=============                 Main Program Start                   =============
00077 //================================================================================
00078 
00079 int main(){
00080     pc.baud(115200);
00081     
00082     Data = fopen("/sd/Data.txt", "w");
00083     closed = 0;
00084     
00085     gyro_init();
00086     
00087     t.start();
00088     timer = t.read_us();
00089 
00090     while(1) {
00091         if(button) {
00092             
00093             if (closed){
00094                 Data = fopen("/sd/Data.txt", "w");
00095                 closed = 0;
00096             }
00097             
00098             DigitalOut myled(LED1);
00099                 
00100             get_Rates();  // Gyro rates
00101 
00102             dt = (t.read_us() - timer) / 1000000.00;
00103             timer = t.read_us();
00104 
00105 
00106 //================================================================================
00107 //=============                   Char Conversion                    =============
00108 //================================================================================
00109 
00110             vx = ((laccx+accx)/2)*dt;
00111             vy = ((laccy+accy)/2)*dt;
00112             
00113             laccx = accx;
00114             laccy = accy;
00115             
00116             xx = ((lvx+vx)/2)*dt;
00117             yy = ((lvy+vy)/2)*dt;
00118             
00119             lvx = vx;
00120             lvy = vy;
00121             
00122             x += xx;
00123             y += yy;
00124             
00125             if (Data == NULL) pc.printf("Unable to write the file \n");  // This chunk for writing to file on SD card
00126             else fprintf(Data, "%f %f %f\r\n",x,y,timer);  // can read float or double not int (only checked these three types)
00127                         
00128         }else{
00129             if(!closed){                                                                                                           
00130                 fclose(Data);
00131                 closed = 1;
00132             }
00133         }
00134     }
00135 }