godai 田中 / Mbed 2 deprecated mpu6050

mpu6050_sensing

Dependencies:   MPU6050 mbed

Fork of mpu6050_test by Simon Garfieldsg

main.cpp@1:4df856c1b9d8, 2015-12-18 (annotated)

Committer:
godai0505
Date:
Fri Dec 18 13:15:16 2015 +0000
Revision:
1:4df856c1b9d8
Parent:
0:84dda456d02c 
mpu6050_sensing;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garfieldsg 0:84dda456d02c 1 #include "mbed.h"
garfieldsg 0:84dda456d02c 2 #include "MPU6050.h"
garfieldsg 0:84dda456d02c 3
garfieldsg 0:84dda456d02c 4 DigitalOut myled(LED1);
garfieldsg 0:84dda456d02c 5 Serial pc(USBTX, USBRX);
garfieldsg 0:84dda456d02c 6 MPU6050 mpu;
godai0505 1:4df856c1b9d8 7 Timer t;
garfieldsg 0:84dda456d02c 8
garfieldsg 0:84dda456d02c 9 int main()
garfieldsg 0:84dda456d02c 10 {
godai0505 1:4df856c1b9d8 11 int16_t ax, ay, az;//acceleration
godai0505 1:4df856c1b9d8 12 int16_t gx, gy, gz;//angular velocity
godai0505 1:4df856c1b9d8 13
godai0505 1:4df856c1b9d8 14 double gx0,gy0,gz0;//angular velocity
godai0505 1:4df856c1b9d8 15 double theta_x,theta_y,theta_z,theta_x0,theta_y0,theta_z0;//angle
godai0505 1:4df856c1b9d8 16
godai0505 1:4df856c1b9d8 17 double p, q, r;
godai0505 1:4df856c1b9d8 18
godai0505 1:4df856c1b9d8 19 double t_read = 0;
godai0505 1:4df856c1b9d8 20
garfieldsg 0:84dda456d02c 21 pc.printf("MPU6050 test\n\n");
garfieldsg 0:84dda456d02c 22 pc.printf("MPU6050 initialize \n");
garfieldsg 0:84dda456d02c 23
garfieldsg 0:84dda456d02c 24 mpu.initialize();
garfieldsg 0:84dda456d02c 25 pc.printf("MPU6050 testConnection \n");
garfieldsg 0:84dda456d02c 26
garfieldsg 0:84dda456d02c 27 bool mpu6050TestResult = mpu.testConnection();
garfieldsg 0:84dda456d02c 28 if(mpu6050TestResult) {
garfieldsg 0:84dda456d02c 29 pc.printf("MPU6050 test passed \n");
garfieldsg 0:84dda456d02c 30 } else {
garfieldsg 0:84dda456d02c 31 pc.printf("MPU6050 test failed \n");
garfieldsg 0:84dda456d02c 32 }
godai0505 1:4df856c1b9d8 33
godai0505 1:4df856c1b9d8 34 /***************calibration***************/
godai0505 1:4df856c1b9d8 35 double sum_x,sum_y,sum_z,ave_x,ave_y,ave_z;
godai0505 1:4df856c1b9d8 36
godai0505 1:4df856c1b9d8 37 sum_x = 0;
godai0505 1:4df856c1b9d8 38 sum_y = 0;
godai0505 1:4df856c1b9d8 39 sum_z = 0;
godai0505 1:4df856c1b9d8 40
godai0505 1:4df856c1b9d8 41 pc.printf("Calibration start ...\nJust a moment.\n");
godai0505 1:4df856c1b9d8 42
godai0505 1:4df856c1b9d8 43 //ドリフト補正(1000回値をとって、その平均値を算出)
godai0505 1:4df856c1b9d8 44 for(int i=0;i<1000;i++){
godai0505 1:4df856c1b9d8 45 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
godai0505 1:4df856c1b9d8 46 sum_x = sum_x+gx;
godai0505 1:4df856c1b9d8 47 sum_y = sum_y+gy;
godai0505 1:4df856c1b9d8 48 sum_z = sum_z+gz;
godai0505 1:4df856c1b9d8 49 wait(0.01);
godai0505 1:4df856c1b9d8 50 }
godai0505 1:4df856c1b9d8 51
godai0505 1:4df856c1b9d8 52 ave_x = sum_x/1000;
godai0505 1:4df856c1b9d8 53 ave_y = sum_y/1000;
godai0505 1:4df856c1b9d8 54 ave_z = sum_z/1000;
godai0505 1:4df856c1b9d8 55
godai0505 1:4df856c1b9d8 56 pc.printf("Calibration result:%f;%f;%f\n", ave_x, ave_y, ave_z);
godai0505 1:4df856c1b9d8 57
godai0505 1:4df856c1b9d8 58 /*************main*************/
godai0505 1:4df856c1b9d8 59 gx0 = 0;
godai0505 1:4df856c1b9d8 60 gy0 = 0;
godai0505 1:4df856c1b9d8 61 gz0 = 0;
godai0505 1:4df856c1b9d8 62 theta_x0 = 0;
godai0505 1:4df856c1b9d8 63 theta_y0 = 0;
godai0505 1:4df856c1b9d8 64 theta_z0 = 0;
godai0505 1:4df856c1b9d8 65
garfieldsg 0:84dda456d02c 66 while(1) {
godai0505 1:4df856c1b9d8 67 //timer start.
godai0505 1:4df856c1b9d8 68 t.start();
garfieldsg 0:84dda456d02c 69 mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
godai0505 1:4df856c1b9d8 70
godai0505 1:4df856c1b9d8 71 //ドリフト補正＆スケーリング:scalling factor = 131[LSB/(°/s)]
godai0505 1:4df856c1b9d8 72 p = ((double)gx-ave_x) / 131;
godai0505 1:4df856c1b9d8 73 q = ((double)gy-ave_y) / 131;
godai0505 1:4df856c1b9d8 74 r = ((double)gz-ave_z) / 131;
godai0505 1:4df856c1b9d8 75
godai0505 1:4df856c1b9d8 76 pc.printf("gx:%7.2f gy:%7.2f gz:%7.2f ",p, q, r);
godai0505 1:4df856c1b9d8 77
godai0505 1:4df856c1b9d8 78 //台形積分
godai0505 1:4df856c1b9d8 79 theta_x = (gx0+p)/2*t_read+theta_x0;
godai0505 1:4df856c1b9d8 80 theta_y = (gy0+q)/2*t_read+theta_y0;
godai0505 1:4df856c1b9d8 81 theta_z = (gz0+r)/2*t_read+theta_z0;
godai0505 1:4df856c1b9d8 82
godai0505 1:4df856c1b9d8 83 pc.printf("theta_x:%7.0f theta_y:%7.0f theta_z:%7.0f\n",theta_x,theta_y,theta_z);
godai0505 1:4df856c1b9d8 84
godai0505 1:4df856c1b9d8 85 gx0 = p;
godai0505 1:4df856c1b9d8 86 gy0 = q;
godai0505 1:4df856c1b9d8 87 gz0 = r;
godai0505 1:4df856c1b9d8 88 theta_x0 = theta_x;
godai0505 1:4df856c1b9d8 89 theta_y0 = theta_y;
godai0505 1:4df856c1b9d8 90 theta_z0 = theta_z;
godai0505 1:4df856c1b9d8 91
godai0505 1:4df856c1b9d8 92 wait(0.1);
godai0505 1:4df856c1b9d8 93
godai0505 1:4df856c1b9d8 94 //timer reset & stop.
godai0505 1:4df856c1b9d8 95 t.stop();
godai0505 1:4df856c1b9d8 96 t_read = t.read();
godai0505 1:4df856c1b9d8 97 t.reset();
garfieldsg 0:84dda456d02c 98 }
garfieldsg 0:84dda456d02c 99 }