Diff: main.cpp

Revision:

1:54b66b7ca11e

Parent:

0:ecc07e53ba65

--- a/main.cpp	Tue May 31 04:52:50 2016 +0000
+++ b/main.cpp	Wed Jun 15 22:30:35 2016 +0000
@@ -1,11 +1,269 @@
-#include "mbed.h"
-
-Timer t;
-
-int main()
-{
-    t.start();
-    printf("Hello World!\n");
-    t.stop();
-    printf("The time taken was %f seconds\n", t.read());
-}
+// 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);}
\ No newline at end of file