mbed project / Mbed 2 deprecated Hexcopter_Distance_FRDM_IMU_GY-88

Hexcopter distance measurement using IMU unit

Dependencies:   mbed MPU6050_acc_CF ledControl

main.cpp@1:54b66b7ca11e, 2016-06-15 (annotated)

Committer:
mbedproject
Date:
Wed Jun 15 22:30:35 2016 +0000
Revision:
1:54b66b7ca11e
Parent:
0:ecc07e53ba65 
Hexcopter_IMU_distance_v1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedproject 1:54b66b7ca11e 1 // Program by Suraj Vatsya
mbedproject 1:54b66b7ca11e 2 // Special thanks to @author: Baser Kandehir whose library is used to make this program
mbedproject 1:54b66b7ca11e 3 // and to the original MPU6050 library creater Kris Winer's.
mbedproject 1:54b66b7ca11e 4 // This program measure distance with the help of IMU
mbedproject 1:54b66b7ca11e 5 // Only when motion is in one dimension i.e. along X axis
mbedproject 1:54b66b7ca11e 6
mbedproject 1:54b66b7ca11e 7 #include "mbed.h"
mbedproject 1:54b66b7ca11e 8 #include "MPU6050.h"
mbedproject 1:54b66b7ca11e 9 #include "ledControl.h"
mbedproject 1:54b66b7ca11e 10
mbedproject 1:54b66b7ca11e 11 // initialize serial Communication
mbedproject 1:54b66b7ca11e 12 Serial pc(USBTX,USBRX); // default baud rate: 9600
mbedproject 1:54b66b7ca11e 13 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
mbedproject 1:54b66b7ca11e 14 Ticker toggler1;
mbedproject 1:54b66b7ca11e 15 Ticker filter;
mbedproject 1:54b66b7ca11e 16
mbedproject 1:54b66b7ca11e 17 void compFilter();
mbedproject 1:54b66b7ca11e 18
mbedproject 1:54b66b7ca11e 19 //Timer defined
mbedproject 1:54b66b7ca11e 20 Timer t;
mbedproject 1:54b66b7ca11e 21
mbedproject 1:54b66b7ca11e 22 int s, p, m, q = 0;
mbedproject 1:54b66b7ca11e 23 int i, r, u =0;
mbedproject 1:54b66b7ca11e 24 int flag = 0;
mbedproject 1:54b66b7ca11e 25
mbedproject 1:54b66b7ca11e 26
mbedproject 1:54b66b7ca11e 27 float sTotal = 0;
mbedproject 1:54b66b7ca11e 28 float initAcc = 0;
mbedproject 1:54b66b7ca11e 29
mbedproject 1:54b66b7ca11e 30 float sAvg[3] = {0};
mbedproject 1:54b66b7ca11e 31 float sData[1000] = {0};
mbedproject 1:54b66b7ca11e 32 float dData[1000] = {0};
mbedproject 1:54b66b7ca11e 33 float mData[1000] = {0};
mbedproject 1:54b66b7ca11e 34 float tData[1000] = {0};
mbedproject 1:54b66b7ca11e 35 float pos[1000] = {0};
mbedproject 1:54b66b7ca11e 36 float dD[1000] = {0};
mbedproject 1:54b66b7ca11e 37
mbedproject 1:54b66b7ca11e 38 float mTotal = 0;
mbedproject 1:54b66b7ca11e 39 float sumAcc = 0;
mbedproject 1:54b66b7ca11e 40 float avgAcc = 0;
mbedproject 1:54b66b7ca11e 41 float motion = 0;
mbedproject 1:54b66b7ca11e 42 float ac = 0;
mbedproject 1:54b66b7ca11e 43 float realAcc =0;
mbedproject 1:54b66b7ca11e 44 float vel = 0;
mbedproject 1:54b66b7ca11e 45
mbedproject 1:54b66b7ca11e 46 float timeTaken =0;
mbedproject 1:54b66b7ca11e 47 double dis = 0;
mbedproject 1:54b66b7ca11e 48 double dis_m = 0;
mbedproject 1:54b66b7ca11e 49 float tim=0;
mbedproject 1:54b66b7ca11e 50 float ti=0;
mbedproject 1:54b66b7ca11e 51
mbedproject 1:54b66b7ca11e 52 float pitchAngle = 0;
mbedproject 1:54b66b7ca11e 53 float rollAngle = 0;
mbedproject 1:54b66b7ca11e 54
mbedproject 1:54b66b7ca11e 55 unsigned int first=0, second = 0;
mbedproject 1:54b66b7ca11e 56
mbedproject 1:54b66b7ca11e 57 int last, last_2, last_3, last_4 = 0;
mbedproject 1:54b66b7ca11e 58
mbedproject 1:54b66b7ca11e 59 unsigned char bit_i=0;
mbedproject 1:54b66b7ca11e 60
mbedproject 1:54b66b7ca11e 61 // Reset all variables
mbedproject 1:54b66b7ca11e 62 void resetall()
mbedproject 1:54b66b7ca11e 63 {
mbedproject 1:54b66b7ca11e 64 m=0;
mbedproject 1:54b66b7ca11e 65 flag = 0;
mbedproject 1:54b66b7ca11e 66 i=0;
mbedproject 1:54b66b7ca11e 67 sTotal = 0;
mbedproject 1:54b66b7ca11e 68 // initAcc = 0;
mbedproject 1:54b66b7ca11e 69 r=0;
mbedproject 1:54b66b7ca11e 70 u=0;
mbedproject 1:54b66b7ca11e 71 sumAcc =0;
mbedproject 1:54b66b7ca11e 72 avgAcc = 0;
mbedproject 1:54b66b7ca11e 73 ac = 0;
mbedproject 1:54b66b7ca11e 74 dis =0;
mbedproject 1:54b66b7ca11e 75 dis_m = 0;
mbedproject 1:54b66b7ca11e 76 ti= 0;
mbedproject 1:54b66b7ca11e 77 tim =0;
mbedproject 1:54b66b7ca11e 78 q=0;
mbedproject 1:54b66b7ca11e 79 }
mbedproject 1:54b66b7ca11e 80
mbedproject 1:54b66b7ca11e 81 // To reset samples
mbedproject 1:54b66b7ca11e 82 void resetsample()
mbedproject 1:54b66b7ca11e 83 {
mbedproject 1:54b66b7ca11e 84 sAvg[0] = 0;
mbedproject 1:54b66b7ca11e 85 sAvg[1] = 0;
mbedproject 1:54b66b7ca11e 86 }
mbedproject 1:54b66b7ca11e 87
mbedproject 1:54b66b7ca11e 88 // To change Negative value
mbedproject 1:54b66b7ca11e 89 void changval()
mbedproject 1:54b66b7ca11e 90 {
mbedproject 1:54b66b7ca11e 91 if(axx<0)
mbedproject 1:54b66b7ca11e 92 {
mbedproject 1:54b66b7ca11e 93 ac = (0 - axx);
mbedproject 1:54b66b7ca11e 94 }
mbedproject 1:54b66b7ca11e 95 else
mbedproject 1:54b66b7ca11e 96 ac = axx;
mbedproject 1:54b66b7ca11e 97 }
mbedproject 1:54b66b7ca11e 98
mbedproject 1:54b66b7ca11e 99 // To collect Samples
mbedproject 1:54b66b7ca11e 100 void callSample(int value)
mbedproject 1:54b66b7ca11e 101 {
mbedproject 1:54b66b7ca11e 102 // for(int v=0;v<2;v++)
mbedproject 1:54b66b7ca11e 103 {
mbedproject 1:54b66b7ca11e 104 // resetsample();
mbedproject 1:54b66b7ca11e 105 for(s=0;s<10;s++)
mbedproject 1:54b66b7ca11e 106 {
mbedproject 1:54b66b7ca11e 107 changval();
mbedproject 1:54b66b7ca11e 108 sData[s] = ac;
mbedproject 1:54b66b7ca11e 109 sTotal += ac;
mbedproject 1:54b66b7ca11e 110 // wait_ms(1);
mbedproject 1:54b66b7ca11e 111 }
mbedproject 1:54b66b7ca11e 112 sAvg[value] = (sTotal)/(10);
mbedproject 1:54b66b7ca11e 113 sTotal = 0;
mbedproject 1:54b66b7ca11e 114 }
mbedproject 1:54b66b7ca11e 115 }
mbedproject 1:54b66b7ca11e 116
mbedproject 1:54b66b7ca11e 117 // To check if there is any motion in the sensor/platform
mbedproject 1:54b66b7ca11e 118 void checkmotion()
mbedproject 1:54b66b7ca11e 119 {
mbedproject 1:54b66b7ca11e 120 q=0;
mbedproject 1:54b66b7ca11e 121 motion = sAvg[2] - initAcc;
mbedproject 1:54b66b7ca11e 122 if(motion>1)
mbedproject 1:54b66b7ca11e 123 {
mbedproject 1:54b66b7ca11e 124 if(i==0){t.start();
mbedproject 1:54b66b7ca11e 125 ledControl(1,1);}
mbedproject 1:54b66b7ca11e 126
mbedproject 1:54b66b7ca11e 127 m=1;
mbedproject 1:54b66b7ca11e 128 {printf("Motion detected*\r");}
mbedproject 1:54b66b7ca11e 129 }
mbedproject 1:54b66b7ca11e 130 }
mbedproject 1:54b66b7ca11e 131
mbedproject 1:54b66b7ca11e 132 // To check if platoform stopped moving
mbedproject 1:54b66b7ca11e 133 // If not; do nothing
mbedproject 1:54b66b7ca11e 134 // If yes; change flag bit m to zero
mbedproject 1:54b66b7ca11e 135 void checkstop()
mbedproject 1:54b66b7ca11e 136 {
mbedproject 1:54b66b7ca11e 137 if((dData[i-1]) < 0.48 && (dData[i-2]) < 0.1 )
mbedproject 1:54b66b7ca11e 138 {
mbedproject 1:54b66b7ca11e 139 m=0;
mbedproject 1:54b66b7ca11e 140 }
mbedproject 1:54b66b7ca11e 141 }
mbedproject 1:54b66b7ca11e 142
mbedproject 1:54b66b7ca11e 143 // To accumulate Accelerometer & Time Readings
mbedproject 1:54b66b7ca11e 144 void grabAcc()
mbedproject 1:54b66b7ca11e 145 {
mbedproject 1:54b66b7ca11e 146 for(i=0; m==1; i++)
mbedproject 1:54b66b7ca11e 147 {
mbedproject 1:54b66b7ca11e 148 for(r=0; r<5; r++)
mbedproject 1:54b66b7ca11e 149 {
mbedproject 1:54b66b7ca11e 150 changval();
mbedproject 1:54b66b7ca11e 151 mData[r] = ac;
mbedproject 1:54b66b7ca11e 152 mTotal += ac;
mbedproject 1:54b66b7ca11e 153 wait_ms(5);
mbedproject 1:54b66b7ca11e 154 }
mbedproject 1:54b66b7ca11e 155
mbedproject 1:54b66b7ca11e 156 dData[i] = ((mTotal)/5)-initAcc;
mbedproject 1:54b66b7ca11e 157 tData[i] = t.read();
mbedproject 1:54b66b7ca11e 158 ledToggle(2);
mbedproject 1:54b66b7ca11e 159 mTotal = 0;
mbedproject 1:54b66b7ca11e 160 if(i>2)
mbedproject 1:54b66b7ca11e 161 {
mbedproject 1:54b66b7ca11e 162 checkstop();
mbedproject 1:54b66b7ca11e 163 }
mbedproject 1:54b66b7ca11e 164
mbedproject 1:54b66b7ca11e 165 }
mbedproject 1:54b66b7ca11e 166 ledControl(3,1);
mbedproject 1:54b66b7ca11e 167 ledControl(2,1);
mbedproject 1:54b66b7ca11e 168 ledControl(1,0);
mbedproject 1:54b66b7ca11e 169 t.stop();
mbedproject 1:54b66b7ca11e 170 flag=1;
mbedproject 1:54b66b7ca11e 171
mbedproject 1:54b66b7ca11e 172 }
mbedproject 1:54b66b7ca11e 173 void calDistance()
mbedproject 1:54b66b7ca11e 174 {
mbedproject 1:54b66b7ca11e 175
mbedproject 1:54b66b7ca11e 176 for(u=0;u<(i-1);u++)
mbedproject 1:54b66b7ca11e 177 {
mbedproject 1:54b66b7ca11e 178 sumAcc += dData[u];
mbedproject 1:54b66b7ca11e 179 }
mbedproject 1:54b66b7ca11e 180 avgAcc = sumAcc / (i-1);
mbedproject 1:54b66b7ca11e 181 timeTaken = tData[i-1] - tData[0];
mbedproject 1:54b66b7ca11e 182 realAcc = avgAcc;
mbedproject 1:54b66b7ca11e 183 dis = 0.5 * realAcc * (timeTaken * timeTaken);
mbedproject 1:54b66b7ca11e 184 dis_m = 39.37 * dis;
mbedproject 1:54b66b7ca11e 185 flag = 1;
mbedproject 1:54b66b7ca11e 186
mbedproject 1:54b66b7ca11e 187 }
mbedproject 1:54b66b7ca11e 188
mbedproject 1:54b66b7ca11e 189 // Initial acceleration value when sensor/platform is stationary and at leveled state
mbedproject 1:54b66b7ca11e 190 void initAccel()
mbedproject 1:54b66b7ca11e 191 {
mbedproject 1:54b66b7ca11e 192 ledControl(1,1);
mbedproject 1:54b66b7ca11e 193 ledControl(2,1);
mbedproject 1:54b66b7ca11e 194 ledControl(3,1);
mbedproject 1:54b66b7ca11e 195 ledControl(4,1);
mbedproject 1:54b66b7ca11e 196 printf("Raise IMU sensor board and wait till initial readings is taken\r\n");
mbedproject 1:54b66b7ca11e 197 wait_ms(500);
mbedproject 1:54b66b7ca11e 198 printf("Grabbing Initial Accelerometer reading in 3\r");
mbedproject 1:54b66b7ca11e 199 ledControl(4,0);
mbedproject 1:54b66b7ca11e 200 wait_ms(500);
mbedproject 1:54b66b7ca11e 201 printf("Grabbing Initial Accelerometer reading in 2\r");
mbedproject 1:54b66b7ca11e 202 ledControl(3,0);
mbedproject 1:54b66b7ca11e 203 wait_ms(500);
mbedproject 1:54b66b7ca11e 204 printf("Grabbing Initial Accelerometer reading in 3\r");
mbedproject 1:54b66b7ca11e 205 ledControl(2,0);
mbedproject 1:54b66b7ca11e 206 wait_ms(400);
mbedproject 1:54b66b7ca11e 207 printf("Process started");
mbedproject 1:54b66b7ca11e 208 callSample(0);
mbedproject 1:54b66b7ca11e 209 wait_ms(5);
mbedproject 1:54b66b7ca11e 210 callSample(1);
mbedproject 1:54b66b7ca11e 211 wait_ms(5);
mbedproject 1:54b66b7ca11e 212 initAcc = (sAvg[1] + sAvg[0])/2;
mbedproject 1:54b66b7ca11e 213 printf("Initial Reading Process Completed\r\n");
mbedproject 1:54b66b7ca11e 214 printf("Motion process Started, Initial Acc = %f \r\n", initAcc);
mbedproject 1:54b66b7ca11e 215 // printf("Motion process Started, 0 = %f 1= %f \r\n", sAvg[1], sAvg[0]);
mbedproject 1:54b66b7ca11e 216 ledControl(1,1);
mbedproject 1:54b66b7ca11e 217 ledControl(2,1);
mbedproject 1:54b66b7ca11e 218 ledControl(3,1);
mbedproject 1:54b66b7ca11e 219 ledControl(4,1);
mbedproject 1:54b66b7ca11e 220 wait_ms(200);
mbedproject 1:54b66b7ca11e 221 }
mbedproject 1:54b66b7ca11e 222
mbedproject 1:54b66b7ca11e 223 // main program
mbedproject 1:54b66b7ca11e 224 int main()
mbedproject 1:54b66b7ca11e 225 {
mbedproject 1:54b66b7ca11e 226 pc.baud(9600);
mbedproject 1:54b66b7ca11e 227 wait_ms(100); // baud rate: 9600
mbedproject 1:54b66b7ca11e 228 mpu6050.checkaddress(); // Communication test: WHO_AM_I register reading
mbedproject 1:54b66b7ca11e 229 wait(1);
mbedproject 1:54b66b7ca11e 230 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
mbedproject 1:54b66b7ca11e 231 pc.printf("Calibration Done. \r\n");
mbedproject 1:54b66b7ca11e 232 wait(0.5);
mbedproject 1:54b66b7ca11e 233 mpu6050.init(); // Initialize the sensor
mbedproject 1:54b66b7ca11e 234 wait(1);
mbedproject 1:54b66b7ca11e 235 pc.printf("IMU is initialized for operation.. \r\n\r\n");
mbedproject 1:54b66b7ca11e 236 wait_ms(500);
mbedproject 1:54b66b7ca11e 237
mbedproject 1:54b66b7ca11e 238 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
mbedproject 1:54b66b7ca11e 239 // axx=ax*9.8f; ayy=ay*9.8f; azz=az*9.8f;
mbedproject 1:54b66b7ca11e 240 initAccel();
mbedproject 1:54b66b7ca11e 241 //printf(" Acceleration (in m/s^2) Time (in seconds) \r\n");
mbedproject 1:54b66b7ca11e 242 while(1)
mbedproject 1:54b66b7ca11e 243 {
mbedproject 1:54b66b7ca11e 244 filter.attach(&compFilter, 0.005); // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
mbedproject 1:54b66b7ca11e 245 // reset();
mbedproject 1:54b66b7ca11e 246 //------------------------Start Collecting Samples of Accelerometer Data---------------------------
mbedproject 1:54b66b7ca11e 247
mbedproject 1:54b66b7ca11e 248 callSample(2);
mbedproject 1:54b66b7ca11e 249 wait_ms(5);
mbedproject 1:54b66b7ca11e 250 callSample(3);
mbedproject 1:54b66b7ca11e 251 wait_ms(5);
mbedproject 1:54b66b7ca11e 252 // printf("Sample 0 = %f Sample 1 = %f Sample 2 = %f \r", sAvg[0], sAvg[1], sAvg[2]);
mbedproject 1:54b66b7ca11e 253 checkmotion();
mbedproject 1:54b66b7ca11e 254 if(m==1)
mbedproject 1:54b66b7ca11e 255 {grabAcc();}
mbedproject 1:54b66b7ca11e 256 if(flag == 1)
mbedproject 1:54b66b7ca11e 257 {
mbedproject 1:54b66b7ca11e 258 calDistance();
mbedproject 1:54b66b7ca11e 259 printf("distance(in Meter) = %2.4f distance(in Inches) = %f \r\n", dis, dis_m);
mbedproject 1:54b66b7ca11e 260 // wait(5);
mbedproject 1:54b66b7ca11e 261 flag =0;
mbedproject 1:54b66b7ca11e 262 }
mbedproject 1:54b66b7ca11e 263 resetall();
mbedproject 1:54b66b7ca11e 264 t.reset();
mbedproject 1:54b66b7ca11e 265
mbedproject 1:54b66b7ca11e 266 }
mbedproject 1:54b66b7ca11e 267 }
mbedproject 1:54b66b7ca11e 268 /* This function is created to avoid address error that caused from Ticker.attach func */
mbedproject 1:54b66b7ca11e 269 void compFilter() {mpu6050.complementaryFilter(&pitchAngle, &rollAngle);}