Johan Beverini / Mbed 2 deprecated Project_5A_J_B_A

projet 5A ensil Johan Bouthayna Annas

Dependencies:   mbed

main.cpp@1:8f6591373cfd, 2017-11-24 (annotated)

Committer:
JohanBeverini
Date:
Fri Nov 24 11:30:50 2017 +0000
Revision:
1:8f6591373cfd
Parent:
0:1d41bd249237
Child:
2:f89067092cef
aa

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 1:8f6591373cfd 8 Ticker t;
JohanBeverini 0:1d41bd249237 9
JohanBeverini 0:1d41bd249237 10 DigitalOut myled(LED1);
JohanBeverini 0:1d41bd249237 11
JohanBeverini 1:8f6591373cfd 12 float alpha, beta, gamma, R11, R12, R13, R21, R22, R23, R31, R32, R33, poidx, poidy, poidz, periode;
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 0:1d41bd249237 37 Now = t.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 0:1d41bd249237 52
JohanBeverini 0:1d41bd249237 53 //sensorX <= accelCount[0];
JohanBeverini 0:1d41bd249237 54 //sensorY <= accelCount[1];
JohanBeverini 0:1d41bd249237 55 //sensorZ <= accelCount[2];
JohanBeverini 0:1d41bd249237 56
JohanBeverini 0:1d41bd249237 57 PC.printf("acceleration in X = %u, or %f g\n", (unsigned int)accelCount[0], ax);
JohanBeverini 0:1d41bd249237 58 PC.printf("acceleration in Y = %u, or %f g\n", (unsigned int)accelCount[1], ay);
JohanBeverini 0:1d41bd249237 59 PC.printf("acceleration in Z = %u, or %f g\n", (unsigned int)accelCount[2], az);
JohanBeverini 0:1d41bd249237 60
JohanBeverini 0:1d41bd249237 61 PC.printf("gyroscope in X = %u, or %f dps\n", (unsigned int)gyroCount[0], gx);
JohanBeverini 0:1d41bd249237 62 PC.printf("gyroscope in Y = %u, or %f dps\n", (unsigned int)gyroCount[1], gy);
JohanBeverini 0:1d41bd249237 63 PC.printf("gyroscope in Z = %u, or %f dps\n", (unsigned int)gyroCount[2], gz);
JohanBeverini 0:1d41bd249237 64
JohanBeverini 0:1d41bd249237 65 PC.printf("temperature = %u, or %f C\n", (unsigned int)tempCount, temperature);
JohanBeverini 1:8f6591373cfd 66 }
JohanBeverini 1:8f6591373cfd 67
JohanBeverini 1:8f6591373cfd 68 void boucle(void){
JohanBeverini 1:8f6591373cfd 69 recup_MPU();
JohanBeverini 1:8f6591373cfd 70 alpha+=gx*periode;
JohanBeverini 1:8f6591373cfd 71 beta+=gy*periode;
JohanBeverini 1:8f6591373cfd 72 gamma+=gz*periode;
JohanBeverini 1:8f6591373cfd 73
JohanBeverini 1:8f6591373cfd 74
JohanBeverini 1:8f6591373cfd 75 }
JohanBeverini 1:8f6591373cfd 76
JohanBeverini 1:8f6591373cfd 77 int main()
JohanBeverini 1:8f6591373cfd 78 {
JohanBeverini 1:8f6591373cfd 79 PC.baud(9600);
JohanBeverini 1:8f6591373cfd 80 PC.printf("Hello World !\n");
JohanBeverini 1:8f6591373cfd 81 BT.baud(9600);
JohanBeverini 1:8f6591373cfd 82 BT.printf("Connection BT\n");
JohanBeverini 1:8f6591373cfd 83
JohanBeverini 1:8f6591373cfd 84 periode=0.1;
JohanBeverini 1:8f6591373cfd 85
JohanBeverini 1:8f6591373cfd 86 alpha=0.0;
JohanBeverini 1:8f6591373cfd 87 beta=0.0;
JohanBeverini 1:8f6591373cfd 88 gamma=0.0;
JohanBeverini 1:8f6591373cfd 89
JohanBeverini 1:8f6591373cfd 90 recup_MPU();
JohanBeverini 1:8f6591373cfd 91 poidx=ax;
JohanBeverini 1:8f6591373cfd 92 poidy=ay;
JohanBeverini 1:8f6591373cfd 93 poidz=az;
JohanBeverini 1:8f6591373cfd 94
JohanBeverini 1:8f6591373cfd 95
JohanBeverini 1:8f6591373cfd 96 t.attach(&boucle, periode);
JohanBeverini 1:8f6591373cfd 97
JohanBeverini 1:8f6591373cfd 98
JohanBeverini 1:8f6591373cfd 99
JohanBeverini 1:8f6591373cfd 100 while(1) {
JohanBeverini 1:8f6591373cfd 101
JohanBeverini 1:8f6591373cfd 102
JohanBeverini 0:1d41bd249237 103 }
JohanBeverini 0:1d41bd249237 104 }