File content as of revision 1:54b66b7ca11e:

// Program by Suraj Vatsya
// Special thanks to @author: Baser Kandehir whose library is used to make this program
// and to the original MPU6050 library creater Kris Winer's.
// This program measure distance with the help of IMU
// Only when motion is in one dimension i.e. along X axis  

#include "mbed.h"
#include "MPU6050.h"
#include "ledControl.h"

// initialize serial Communication 
Serial pc(USBTX,USBRX);    // default baud rate: 9600
MPU6050 mpu6050;           // class: MPU6050, object: mpu6050 
Ticker toggler1;
Ticker filter;           

void compFilter();

//Timer defined
Timer t;

int s, p, m, q = 0;
int i, r, u =0;
int flag = 0;


float sTotal = 0;
float initAcc = 0;

float sAvg[3] = {0};
float sData[1000] = {0};
float dData[1000] = {0};
float mData[1000] = {0};
float tData[1000] = {0};
float pos[1000] = {0};
float dD[1000] = {0};

float mTotal = 0;
float sumAcc = 0;
float avgAcc = 0;
float motion = 0;
float ac = 0; 
float realAcc =0;
float vel = 0;

float timeTaken =0;
double dis = 0;
double dis_m = 0;
float tim=0;
float ti=0;

float pitchAngle = 0;
float rollAngle = 0;

unsigned int first=0, second = 0;

int last, last_2, last_3, last_4 = 0;

unsigned char bit_i=0;

// Reset all variables
void resetall()
{
    m=0;
    flag = 0;
    i=0;
    sTotal = 0;
 //   initAcc = 0;
    r=0;
    u=0;
    sumAcc =0;
    avgAcc = 0;  
    ac = 0;  
    dis =0;
    dis_m = 0;
    ti= 0;
    tim =0;
    q=0;
}

// To reset samples
void resetsample()
{
    sAvg[0] = 0;
    sAvg[1] = 0;        
}

// To change Negative value
void changval()
{
    if(axx<0)
        {
            ac = (0 - axx);    
        }    
    else
    ac = axx;
}

// To collect Samples
void callSample(int value)
{
//    for(int v=0;v<2;v++)
    {
//        resetsample();
        for(s=0;s<10;s++)
            {
                changval();
                sData[s] = ac;    
                sTotal += ac;
//                wait_ms(1);
            }
    sAvg[value] = (sTotal)/(10);
    sTotal = 0;
}
}

// To check if there is any motion in the sensor/platform 
void checkmotion()
{
   q=0;
   motion = sAvg[2] - initAcc;    
    if(motion>1)
        {
             if(i==0){t.start();
             ledControl(1,1);}
             
            m=1;
            {printf("Motion detected*\r");}
        }
}

// To check if platoform stopped moving
// If not; do nothing
// If yes; change flag bit m to zero
void checkstop()
{
    if((dData[i-1]) < 0.48  && (dData[i-2]) < 0.1 )
        {
            m=0;    
        }
}

// To accumulate Accelerometer & Time Readings
void grabAcc()
{
    for(i=0; m==1; i++)
        {
            for(r=0; r<5; r++)
                {
                    changval();
                    mData[r] = ac;    
                    mTotal += ac;
                    wait_ms(5);
                }    
 
            dData[i] = ((mTotal)/5)-initAcc;
            tData[i] = t.read();
            ledToggle(2);
            mTotal = 0;
        if(i>2)
        {
            checkstop();
            }
    
    }
    ledControl(3,1);
    ledControl(2,1);
    ledControl(1,0);
t.stop();     
flag=1;

}
void calDistance()
{
    
    for(u=0;u<(i-1);u++)
        {
            sumAcc += dData[u];    
        }    
    avgAcc = sumAcc / (i-1);
    timeTaken = tData[i-1] - tData[0];
    realAcc = avgAcc;
    dis = 0.5 * realAcc * (timeTaken * timeTaken);
    dis_m = 39.37 * dis;
    flag = 1;
    
}

// Initial acceleration value when sensor/platform is stationary and at leveled state
void initAccel()
{
    ledControl(1,1);
    ledControl(2,1);
    ledControl(3,1);
    ledControl(4,1);
    printf("Raise IMU sensor board and wait till initial readings is taken\r\n");
    wait_ms(500);
    printf("Grabbing Initial Accelerometer reading in 3\r");
    ledControl(4,0);
    wait_ms(500);
    printf("Grabbing Initial Accelerometer reading in 2\r");
    ledControl(3,0);
    wait_ms(500);
    printf("Grabbing Initial Accelerometer reading in 3\r");
    ledControl(2,0);
    wait_ms(400);
    printf("Process started");
    callSample(0);
    wait_ms(5);
    callSample(1);
    wait_ms(5);
    initAcc = (sAvg[1] + sAvg[0])/2;
    printf("Initial Reading Process Completed\r\n");
    printf("Motion process Started, Initial Acc = %f    \r\n", initAcc);
//    printf("Motion process Started, 0 = %f    1= %f     \r\n", sAvg[1], sAvg[0]);
    ledControl(1,1);
    ledControl(2,1);
    ledControl(3,1);
    ledControl(4,1);
    wait_ms(200);
}

// main program
int main() 
{
    pc.baud(9600); 
    wait_ms(100);                             // baud rate: 9600
    mpu6050.checkaddress();                           // Communication test: WHO_AM_I register reading 
    wait(1);
    mpu6050.calibrate(accelBias,gyroBias);      // Calibrate MPU6050 and load biases into bias registers
    pc.printf("Calibration Done. \r\n");
    wait(0.5);
    mpu6050.init();                             // Initialize the sensor
    wait(1);
   pc.printf("IMU is initialized for operation.. \r\n\r\n");
    wait_ms(500);

    filter.attach(&compFilter, 0.005);    // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
//  axx=ax*9.8f;   ayy=ay*9.8f;    azz=az*9.8f;
initAccel(); 
//printf("    Acceleration (in m/s^2)       Time (in seconds) \r\n");
while(1) 
 {
    filter.attach(&compFilter, 0.005);    // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
//    reset();
//------------------------Start Collecting Samples of Accelerometer Data---------------------------
    
    callSample(2);
    wait_ms(5);
    callSample(3);
    wait_ms(5);
//    printf("Sample 0 = %f           Sample 1 =  %f          Sample 2 = %f  \r", sAvg[0], sAvg[1], sAvg[2]);
    checkmotion();
    if(m==1)
    {grabAcc();}
    if(flag == 1)
        {
        calDistance();
        printf("distance(in Meter) = %2.4f      distance(in Inches) = %f \r\n", dis, dis_m);
//        wait(5);
        flag =0;
        }
        resetall();
        t.reset();

}
}
/* This function is created to avoid address error that caused from Ticker.attach func */ 
void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}