Johan Beverini / Mbed 2 deprecated Project_5A_J_B_A

projet 5A ensil Johan Bouthayna Annas

Dependencies:   mbed

main.cpp@2:f89067092cef, 2018-01-18 (annotated)

Committer:
JohanBeverini
Date:
Thu Jan 18 13:50:34 2018 +0000
Revision:
2:f89067092cef
Parent:
1:8f6591373cfd 
version apres noel

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JohanBeverini 0:1d41bd249237 1 #include "mbed.h"
JohanBeverini 0:1d41bd249237 2 #include "MPU6050.h"
JohanBeverini 0:1d41bd249237 3
JohanBeverini 0:1d41bd249237 4 Serial PC(SERIAL_TX, SERIAL_RX);
JohanBeverini 2:f89067092cef 5 Serial BT(PA_9, PA_10);
JohanBeverini 0:1d41bd249237 6 MPU6050 mpu6050;
JohanBeverini 1:8f6591373cfd 7 Ticker t;
JohanBeverini 2:f89067092cef 8 Timer t1;
JohanBeverini 0:1d41bd249237 9
JohanBeverini 0:1d41bd249237 10 DigitalOut myled(LED1);
JohanBeverini 0:1d41bd249237 11
JohanBeverini 2:f89067092cef 12 float alpha, betaa, gammaa, R11, R12, R13, R21, R22, R23, R31, R32, R33, poidx, poidy, poidz, periode, sumCount, sum;
JohanBeverini 1:8f6591373cfd 13
JohanBeverini 1:8f6591373cfd 14 void recup_MPU(void){
JohanBeverini 0:1d41bd249237 15
JohanBeverini 0:1d41bd249237 16 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
JohanBeverini 0:1d41bd249237 17 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
JohanBeverini 0:1d41bd249237 18 mpu6050.getAres();
JohanBeverini 0:1d41bd249237 19
JohanBeverini 0:1d41bd249237 20 // Now we'll calculate the accleration value into actual g's
JohanBeverini 0:1d41bd249237 21 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
JohanBeverini 0:1d41bd249237 22 ay = (float)accelCount[1]*aRes - accelBias[1];
JohanBeverini 0:1d41bd249237 23 az = (float)accelCount[2]*aRes - accelBias[2];
JohanBeverini 0:1d41bd249237 24
JohanBeverini 0:1d41bd249237 25 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
JohanBeverini 0:1d41bd249237 26 mpu6050.getGres();
JohanBeverini 0:1d41bd249237 27
JohanBeverini 0:1d41bd249237 28 // Calculate the gyro value into actual degrees per second
JohanBeverini 0:1d41bd249237 29 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
JohanBeverini 0:1d41bd249237 30 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
JohanBeverini 0:1d41bd249237 31 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
JohanBeverini 0:1d41bd249237 32
JohanBeverini 0:1d41bd249237 33 tempCount = mpu6050.readTempData(); // Read the adc values
JohanBeverini 0:1d41bd249237 34 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
JohanBeverini 0:1d41bd249237 35 }
JohanBeverini 0:1d41bd249237 36
JohanBeverini 2:f89067092cef 37 Now = t1.read_us();
JohanBeverini 0:1d41bd249237 38 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
JohanBeverini 0:1d41bd249237 39 lastUpdate = Now;
JohanBeverini 0:1d41bd249237 40
JohanBeverini 0:1d41bd249237 41 sum += deltat;
JohanBeverini 0:1d41bd249237 42 sumCount++;
JohanBeverini 0:1d41bd249237 43
JohanBeverini 0:1d41bd249237 44 if(lastUpdate - firstUpdate > 10000000.0f) {
JohanBeverini 0:1d41bd249237 45 beta = 0.04; // decrease filter gain after stabilized
JohanBeverini 0:1d41bd249237 46 zeta = 0.015; // increasey bias drift gain after stabilized
JohanBeverini 0:1d41bd249237 47 }
JohanBeverini 0:1d41bd249237 48
JohanBeverini 0:1d41bd249237 49 // Pass gyro rate as rad/s
JohanBeverini 0:1d41bd249237 50 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
JohanBeverini 0:1d41bd249237 51
JohanBeverini 2:f89067092cef 52 (gx,gy,gz)=(gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
JohanBeverini 0:1d41bd249237 53
JohanBeverini 0:1d41bd249237 54 //sensorX <= accelCount[0];
JohanBeverini 0:1d41bd249237 55 //sensorY <= accelCount[1];
JohanBeverini 0:1d41bd249237 56 //sensorZ <= accelCount[2];
JohanBeverini 0:1d41bd249237 57
JohanBeverini 0:1d41bd249237 58 PC.printf("acceleration in X = %u, or %f g\n", (unsigned int)accelCount[0], ax);
JohanBeverini 0:1d41bd249237 59 PC.printf("acceleration in Y = %u, or %f g\n", (unsigned int)accelCount[1], ay);
JohanBeverini 0:1d41bd249237 60 PC.printf("acceleration in Z = %u, or %f g\n", (unsigned int)accelCount[2], az);
JohanBeverini 0:1d41bd249237 61
JohanBeverini 2:f89067092cef 62 PC.printf("gyroscope in X = %u, or %f rad/s\n", (unsigned int)gyroCount[0], gx);
JohanBeverini 2:f89067092cef 63 PC.printf("gyroscope in Y = %u, or %f rad/s\n", (unsigned int)gyroCount[1], gy);
JohanBeverini 2:f89067092cef 64 PC.printf("gyroscope in Z = %u, or %f rad/s\n", (unsigned int)gyroCount[2], gz);
JohanBeverini 0:1d41bd249237 65
JohanBeverini 0:1d41bd249237 66 PC.printf("temperature = %u, or %f C\n", (unsigned int)tempCount, temperature);
JohanBeverini 1:8f6591373cfd 67 }
JohanBeverini 1:8f6591373cfd 68
JohanBeverini 2:f89067092cef 69
JohanBeverini 2:f89067092cef 70
JohanBeverini 2:f89067092cef 71
JohanBeverini 2:f89067092cef 72
JohanBeverini 1:8f6591373cfd 73 void boucle(void){
JohanBeverini 1:8f6591373cfd 74 recup_MPU();
JohanBeverini 1:8f6591373cfd 75 alpha+=gx*periode;
JohanBeverini 2:f89067092cef 76 betaa+=gy*periode;
JohanBeverini 2:f89067092cef 77 gammaa+=gz*periode;
JohanBeverini 1:8f6591373cfd 78
JohanBeverini 2:f89067092cef 79 BT.printf("acceleration in X = %u, or %f g\n", (unsigned int)accelCount[0], ax);
JohanBeverini 2:f89067092cef 80
JohanBeverini 2:f89067092cef 81 myled!=myled;
JohanBeverini 1:8f6591373cfd 82
JohanBeverini 1:8f6591373cfd 83 }
JohanBeverini 1:8f6591373cfd 84
JohanBeverini 2:f89067092cef 85
JohanBeverini 2:f89067092cef 86
JohanBeverini 2:f89067092cef 87
JohanBeverini 2:f89067092cef 88
JohanBeverini 1:8f6591373cfd 89 int main()
JohanBeverini 1:8f6591373cfd 90 {
JohanBeverini 1:8f6591373cfd 91 PC.baud(9600);
JohanBeverini 1:8f6591373cfd 92 PC.printf("Hello World !\n");
JohanBeverini 2:f89067092cef 93 BT.baud(38400);
JohanBeverini 1:8f6591373cfd 94 BT.printf("Connection BT\n");
JohanBeverini 1:8f6591373cfd 95
JohanBeverini 2:f89067092cef 96 periode=1;
JohanBeverini 1:8f6591373cfd 97
JohanBeverini 1:8f6591373cfd 98 alpha=0.0;
JohanBeverini 2:f89067092cef 99 betaa=0.0;
JohanBeverini 2:f89067092cef 100 gammaa=0.0;
JohanBeverini 2:f89067092cef 101
JohanBeverini 2:f89067092cef 102
JohanBeverini 2:f89067092cef 103
JohanBeverini 2:f89067092cef 104
JohanBeverini 2:f89067092cef 105
JohanBeverini 2:f89067092cef 106 /////////////////////////////////////////////////////////////////////////////////////////////////
JohanBeverini 2:f89067092cef 107 // Read the WHO_AM_I register, this is a good test of communication
JohanBeverini 2:f89067092cef 108 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
JohanBeverini 2:f89067092cef 109 PC.printf("I AM 0x%x\n\r", whoami);
JohanBeverini 2:f89067092cef 110 PC.printf("I SHOULD BE 0x68\n\r");
JohanBeverini 2:f89067092cef 111
JohanBeverini 2:f89067092cef 112 if (whoami == 0x68) // WHO_AM_I should always be 0x68
JohanBeverini 2:f89067092cef 113 {
JohanBeverini 2:f89067092cef 114 PC.printf("MPU6050 is online...");
JohanBeverini 2:f89067092cef 115 wait(1);
JohanBeverini 2:f89067092cef 116
JohanBeverini 2:f89067092cef 117 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
JohanBeverini 2:f89067092cef 118 PC.printf("x-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[0]); PC.printf("% of factory value \n\r");
JohanBeverini 2:f89067092cef 119 PC.printf("y-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[1]); PC.printf("% of factory value \n\r");
JohanBeverini 2:f89067092cef 120 PC.printf("z-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[2]); PC.printf("% of factory value \n\r");
JohanBeverini 2:f89067092cef 121 PC.printf("x-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[3]); PC.printf("% of factory value \n\r");
JohanBeverini 2:f89067092cef 122 PC.printf("y-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[4]); PC.printf("% of factory value \n\r");
JohanBeverini 2:f89067092cef 123 PC.printf("z-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[5]); PC.printf("% of factory value \n\r");
JohanBeverini 2:f89067092cef 124 wait(1);
JohanBeverini 2:f89067092cef 125
JohanBeverini 2:f89067092cef 126 if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f)
JohanBeverini 2:f89067092cef 127 {
JohanBeverini 2:f89067092cef 128 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
JohanBeverini 2:f89067092cef 129 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
JohanBeverini 2:f89067092cef 130 mpu6050.initMPU6050(); PC.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
JohanBeverini 2:f89067092cef 131
JohanBeverini 2:f89067092cef 132 wait(2);
JohanBeverini 2:f89067092cef 133 }
JohanBeverini 2:f89067092cef 134 else
JohanBeverini 2:f89067092cef 135 {
JohanBeverini 2:f89067092cef 136 PC.printf("Device did not the pass self-test!\n\r");
JohanBeverini 2:f89067092cef 137
JohanBeverini 2:f89067092cef 138 }
JohanBeverini 2:f89067092cef 139 }
JohanBeverini 2:f89067092cef 140 else
JohanBeverini 2:f89067092cef 141 {
JohanBeverini 2:f89067092cef 142 PC.printf("Could not connect to MPU6050: \n\r");
JohanBeverini 2:f89067092cef 143 PC.printf("%#x \n", whoami);
JohanBeverini 2:f89067092cef 144
JohanBeverini 2:f89067092cef 145 while(1) ; // Loop forever if communication doesn't happen
JohanBeverini 2:f89067092cef 146 }
JohanBeverini 2:f89067092cef 147
JohanBeverini 2:f89067092cef 148 PC.printf("init sensor done\n");
JohanBeverini 2:f89067092cef 149 /////////////////////////////////////////////////////////////////////////////////////
JohanBeverini 2:f89067092cef 150
JohanBeverini 2:f89067092cef 151
JohanBeverini 2:f89067092cef 152
JohanBeverini 2:f89067092cef 153
JohanBeverini 2:f89067092cef 154
JohanBeverini 2:f89067092cef 155 t1.start();
JohanBeverini 1:8f6591373cfd 156
JohanBeverini 1:8f6591373cfd 157 recup_MPU();
JohanBeverini 1:8f6591373cfd 158 poidx=ax;
JohanBeverini 1:8f6591373cfd 159 poidy=ay;
JohanBeverini 1:8f6591373cfd 160 poidz=az;
JohanBeverini 1:8f6591373cfd 161
JohanBeverini 1:8f6591373cfd 162
JohanBeverini 1:8f6591373cfd 163 t.attach(&boucle, periode);
JohanBeverini 1:8f6591373cfd 164
JohanBeverini 1:8f6591373cfd 165
JohanBeverini 1:8f6591373cfd 166
JohanBeverini 2:f89067092cef 167
JohanBeverini 1:8f6591373cfd 168 while(1) {
JohanBeverini 1:8f6591373cfd 169
JohanBeverini 2:f89067092cef 170 char c = BT.getc();
JohanBeverini 2:f89067092cef 171
JohanBeverini 2:f89067092cef 172 if(c == 'a') {
JohanBeverini 2:f89067092cef 173 BT.printf("\nOK\n");
JohanBeverini 2:f89067092cef 174 }
JohanBeverini 1:8f6591373cfd 175
JohanBeverini 0:1d41bd249237 176 }
JohanBeverini 0:1d41bd249237 177 }