Hexcopter distance measurement using IMU unit
Dependencies: mbed MPU6050_acc_CF ledControl
Revision 1:54b66b7ca11e, committed 2016-06-15
- Comitter:
- mbedproject
- Date:
- Wed Jun 15 22:30:35 2016 +0000
- Parent:
- 0:ecc07e53ba65
- Commit message:
- Hexcopter_IMU_distance_v1
Changed in this revision
diff -r ecc07e53ba65 -r 54b66b7ca11e MPU6050.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.lib Wed Jun 15 22:30:35 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/mbedproject/code/MPU6050_acc_CF/#0e3519559bcb
diff -r ecc07e53ba65 -r 54b66b7ca11e ledControl.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ledControl.lib Wed Jun 15 22:30:35 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/mbedproject/code/ledControl/#122c7789a056
diff -r ecc07e53ba65 -r 54b66b7ca11e main.cpp --- 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