Johan Beverini / Mbed 2 deprecated Project_5A_J_B_A

projet 5A ensil Johan Bouthayna Annas

Dependencies:   mbed

main.cpp@0:1d41bd249237, 2017-11-24 (annotated)

Committer:
JohanBeverini
Date:
Fri Nov 24 11:10:42 2017 +0000
Revision:
0:1d41bd249237
Child:
1:8f6591373cfd
projet de 5A ensil; Johan, Bouthayna, Annas

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 0:1d41bd249237 5 Serial BT(PA_10, PA_9);
JohanBeverini 0:1d41bd249237 6
JohanBeverini 0:1d41bd249237 7 MPU6050 mpu6050;
JohanBeverini 0:1d41bd249237 8
JohanBeverini 0:1d41bd249237 9 DigitalOut myled(LED1);
JohanBeverini 0:1d41bd249237 10
JohanBeverini 0:1d41bd249237 11 int main()
JohanBeverini 0:1d41bd249237 12 {
JohanBeverini 0:1d41bd249237 13 PC.baud(9600);
JohanBeverini 0:1d41bd249237 14 PC.printf("Hello World !\n");
JohanBeverini 0:1d41bd249237 15 BT.baud(9600);
JohanBeverini 0:1d41bd249237 16 BT.printf("Connection BT\n");
JohanBeverini 0:1d41bd249237 17
JohanBeverini 0:1d41bd249237 18
JohanBeverini 0:1d41bd249237 19
JohanBeverini 0:1d41bd249237 20
JohanBeverini 0:1d41bd249237 21 while(1) {
JohanBeverini 0:1d41bd249237 22
JohanBeverini 0:1d41bd249237 23 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
JohanBeverini 0:1d41bd249237 24 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
JohanBeverini 0:1d41bd249237 25 mpu6050.getAres();
JohanBeverini 0:1d41bd249237 26
JohanBeverini 0:1d41bd249237 27 // Now we'll calculate the accleration value into actual g's
JohanBeverini 0:1d41bd249237 28 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
JohanBeverini 0:1d41bd249237 29 ay = (float)accelCount[1]*aRes - accelBias[1];
JohanBeverini 0:1d41bd249237 30 az = (float)accelCount[2]*aRes - accelBias[2];
JohanBeverini 0:1d41bd249237 31
JohanBeverini 0:1d41bd249237 32 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
JohanBeverini 0:1d41bd249237 33 mpu6050.getGres();
JohanBeverini 0:1d41bd249237 34
JohanBeverini 0:1d41bd249237 35 // Calculate the gyro value into actual degrees per second
JohanBeverini 0:1d41bd249237 36 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
JohanBeverini 0:1d41bd249237 37 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
JohanBeverini 0:1d41bd249237 38 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
JohanBeverini 0:1d41bd249237 39
JohanBeverini 0:1d41bd249237 40 tempCount = mpu6050.readTempData(); // Read the adc values
JohanBeverini 0:1d41bd249237 41 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
JohanBeverini 0:1d41bd249237 42 }
JohanBeverini 0:1d41bd249237 43
JohanBeverini 0:1d41bd249237 44 Now = t.read_us();
JohanBeverini 0:1d41bd249237 45 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
JohanBeverini 0:1d41bd249237 46 lastUpdate = Now;
JohanBeverini 0:1d41bd249237 47
JohanBeverini 0:1d41bd249237 48 sum += deltat;
JohanBeverini 0:1d41bd249237 49 sumCount++;
JohanBeverini 0:1d41bd249237 50
JohanBeverini 0:1d41bd249237 51 if(lastUpdate - firstUpdate > 10000000.0f) {
JohanBeverini 0:1d41bd249237 52 beta = 0.04; // decrease filter gain after stabilized
JohanBeverini 0:1d41bd249237 53 zeta = 0.015; // increasey bias drift gain after stabilized
JohanBeverini 0:1d41bd249237 54 }
JohanBeverini 0:1d41bd249237 55
JohanBeverini 0:1d41bd249237 56 // Pass gyro rate as rad/s
JohanBeverini 0:1d41bd249237 57 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
JohanBeverini 0:1d41bd249237 58
JohanBeverini 0:1d41bd249237 59
JohanBeverini 0:1d41bd249237 60 //sensorX <= accelCount[0];
JohanBeverini 0:1d41bd249237 61 //sensorY <= accelCount[1];
JohanBeverini 0:1d41bd249237 62 //sensorZ <= accelCount[2];
JohanBeverini 0:1d41bd249237 63
JohanBeverini 0:1d41bd249237 64 PC.printf("acceleration in X = %u, or %f g\n", (unsigned int)accelCount[0], ax);
JohanBeverini 0:1d41bd249237 65 PC.printf("acceleration in Y = %u, or %f g\n", (unsigned int)accelCount[1], ay);
JohanBeverini 0:1d41bd249237 66 PC.printf("acceleration in Z = %u, or %f g\n", (unsigned int)accelCount[2], az);
JohanBeverini 0:1d41bd249237 67
JohanBeverini 0:1d41bd249237 68 PC.printf("gyroscope in X = %u, or %f dps\n", (unsigned int)gyroCount[0], gx);
JohanBeverini 0:1d41bd249237 69 PC.printf("gyroscope in Y = %u, or %f dps\n", (unsigned int)gyroCount[1], gy);
JohanBeverini 0:1d41bd249237 70 PC.printf("gyroscope in Z = %u, or %f dps\n", (unsigned int)gyroCount[2], gz);
JohanBeverini 0:1d41bd249237 71
JohanBeverini 0:1d41bd249237 72 PC.printf("temperature = %u, or %f C\n", (unsigned int)tempCount, temperature);
JohanBeverini 0:1d41bd249237 73 }
JohanBeverini 0:1d41bd249237 74 }