File content as of revision 0:9ff166992591:

#include "MPU6050.h"
#include "mbed.h"
#include "SDFileSystem.h"

SDFileSystem sd(PTE3, PTE1, PTE2, PTE4, "sd"); // MOSI, MISO, SCK, CS

Serial pc(USBTX, USBRX);

FILE *Data;   // Creates name of file
char buffer[5000000];  // Buffer size (# of characters) for SD card

MPU6050 mpu6050;
Timer t;

DigitalIn button(PTB9);

float accx,accy,laccx=0,laccy=0;
float vx,vy,lvx=0,lvy=0;
float x,xx,y,yy;
double dt, timer;

bool closed = 1;    // Flag for sd card, to open and close with switch


//================================================================================
//=============               Sensor Initialization                  =============
//================================================================================

void gyro_init(){
    i2c.frequency(400000);
    
    uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
    pc.printf("I AM 0x%x\n\r", whoami);
    pc.printf("I SHOULD BE 0x68\n\r");

    if (whoami == 0x68){
        pc.printf("MPU6050 is online...");
        wait(1);

        mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
        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) {
            mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
            mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
            mpu6050.initMPU6050();
            pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

            pc.printf("\nMPU6050 passed self test... Initializing");
            wait(2); //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        } else pc.printf("\nDevice did not the pass self-test!\n\r");
    } else {
        pc.printf("Could not connect to MPU6050: \n\r");
        pc.printf("%#x \n",  whoami);
        while(1) ; // Loop forever if communication doesn't happen
    }
}


//================================================================================
//=============                     Get Inputs                       =============
//================================================================================

void get_Rates(){
    if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
        mpu6050.readAccelData(accelCount);  // Read the x/y/z adc accel values
        mpu6050.getAres();
        accy = -(accelCount[0]*aRes - accelBias[0]); // g val
        accx = accelCount[1]*aRes - accelBias[1];   
        
        accx /= 9.81;
        accy /= 9.81;
    }
}


//================================================================================
//=============                 Main Program Start                   =============
//================================================================================

int main(){
    pc.baud(115200);
    
    Data = fopen("/sd/Data.txt", "w");
    closed = 0;
    
    gyro_init();
    
    t.start();
    timer = t.read_us();

    while(1) {
        if(button) {
            
            if (closed){
                Data = fopen("/sd/Data.txt", "w");
                closed = 0;
            }
            
            DigitalOut myled(LED1);
                
            get_Rates();  // Gyro rates

            dt = (t.read_us() - timer) / 1000000.00;
            timer = t.read_us();


//================================================================================
//=============                   Char Conversion                    =============
//================================================================================

            vx = ((laccx+accx)/2)*dt;
            vy = ((laccy+accy)/2)*dt;
            
            laccx = accx;
            laccy = accy;
            
            xx = ((lvx+vx)/2)*dt;
            yy = ((lvy+vy)/2)*dt;
            
            lvx = vx;
            lvy = vy;
            
            x += xx;
            y += yy;
            
            if (Data == NULL) pc.printf("Unable to write the file \n");  // This chunk for writing to file on SD card
            else fprintf(Data, "%f %f %f\r\n",x,y,timer);  // can read float or double not int (only checked these three types)
                        
        }else{
            if(!closed){                                                                                                           
                fclose(Data);
                closed = 1;
            }
        }
    }
}