TheBeauGosse / Mbed 2 deprecated ALTIMU_v6

Accelero Brindan

Dependencies:   mbed LSM6DS33

main.cpp@2:59eeb8637e5d, 2019-05-09 (annotated)

Committer:
Birunthan
Date:
Thu May 09 21:02:26 2019 +0000
Revision:
2:59eeb8637e5d
Parent:
1:bcb2d1a61147 
Pour le finale !

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaxLaMenace 0:a2a605398a75 1 #include "LSM6DS33.h"
MaxLaMenace 0:a2a605398a75 2 #include <stdio.h>
MaxLaMenace 0:a2a605398a75 3 #include <stdlib.h>
MaxLaMenace 0:a2a605398a75 4
MaxLaMenace 1:bcb2d1a61147 5
MaxLaMenace 1:bcb2d1a61147 6 // Definition des variables de l'accéléromètre
MaxLaMenace 1:bcb2d1a61147 7
MaxLaMenace 1:bcb2d1a61147 8 LSM6DS33 acc = LSM6DS33(I2C_SDA, I2C_SCL); //Déclarations des broches pour la communication en I2C
MaxLaMenace 0:a2a605398a75 9
Birunthan 2:59eeb8637e5d 10 float axx0; // On définit la variable pour l'accélération initiale en x
Birunthan 2:59eeb8637e5d 11 float ayy0; // On définit la variable pour l'accélération initiale en y
MaxLaMenace 0:a2a605398a75 12
Birunthan 2:59eeb8637e5d 13 float axx1; // On définit la variable pour l'accélération évoluant au cours du temps selon x
Birunthan 2:59eeb8637e5d 14 float ayy1; // On définit la variable pour l'accélération évoluant au cours du temps selon y
MaxLaMenace 0:a2a605398a75 15
Birunthan 2:59eeb8637e5d 16 float vx0; // On définit la variable pour la vitesse initiale en x
Birunthan 2:59eeb8637e5d 17 float vy0; // On définit la variable pour la vitesse initiale en y
MaxLaMenace 0:a2a605398a75 18
Birunthan 2:59eeb8637e5d 19 float vx; // On définit la variable pour la vitesse évoluant au cours du temps selon x
Birunthan 2:59eeb8637e5d 20 float vy; // On définit la variable pour la vitesse évoluant au cours du temps selon y
Birunthan 2:59eeb8637e5d 21 float V; // On définit la variable pour la norme de la vitesse.
MaxLaMenace 0:a2a605398a75 22
Birunthan 2:59eeb8637e5d 23 float epsilonx; // Variable utilisé pour l'initialisation, elle permet d'avoir la valeur de axx0 la plus proche possible de l'accélération initiale en x
Birunthan 2:59eeb8637e5d 24 float epsilony; // Variable utilisé pour l'initialisation, elle permet d'avoir la valeur de ayy0 la plus proche possible de l'accélération initiale en y
Birunthan 2:59eeb8637e5d 25 // En effet, lors de la mesure de l'accélération initiale, on ne peut pas être certains que la première mesure faite par l'accéléromètre correspond
Birunthan 2:59eeb8637e5d 26 // exactement à l'accélération intiale : il est possible qu'un léger décalage/rotation ait eu lieu, résultant à une accélération intiale erronée.
Birunthan 2:59eeb8637e5d 27 // Par la suite, on explique la méthode utilisée pour résoudre ce problème
MaxLaMenace 0:a2a605398a75 28
Birunthan 2:59eeb8637e5d 29 float tabax[5]; // Tableau des 5 dernières accélérations en x mesurées
Birunthan 2:59eeb8637e5d 30 float tabay[5]; // Tableau des 5 dernières accélérations en y mesurées
Birunthan 2:59eeb8637e5d 31 float tabvx[5]; // Tableau des 5 dernières vitesses en x mesurées
Birunthan 2:59eeb8637e5d 32 float tabvy[5]; // Tableau des 5 dernières vitesses en y mesurées
MaxLaMenace 0:a2a605398a75 33
Birunthan 2:59eeb8637e5d 34 int k; // Variable d'incrémentation
MaxLaMenace 0:a2a605398a75 35
MaxLaMenace 0:a2a605398a75 36
MaxLaMenace 0:a2a605398a75 37 int main()
MaxLaMenace 0:a2a605398a75 38 {
Birunthan 2:59eeb8637e5d 39 // On initialise les différentes variables qui évoluent :
Birunthan 2:59eeb8637e5d 40 k = 0; // l'entier k nous permettant d'indexer les mesures
Birunthan 2:59eeb8637e5d 41 vx = 0; // la vitesse en x
Birunthan 2:59eeb8637e5d 42 vy = 0; // la vitesse en y
MaxLaMenace 0:a2a605398a75 43
Birunthan 2:59eeb8637e5d 44 acc.begin(); // On démarre la lecture de l'accéléromètre
MaxLaMenace 0:a2a605398a75 45
Birunthan 2:59eeb8637e5d 46 acc.readAccel(); // On lit les données de l'accéléromètre
Birunthan 2:59eeb8637e5d 47 axx0 = acc.ax; // On affecte à axx0 la première lecture par l'accéléromètre selon l'axe x
Birunthan 2:59eeb8637e5d 48 ayy0 = acc.ay; // On affecte à ayy0 la première lecture par l'accéléromètre selon l'axe y
MaxLaMenace 0:a2a605398a75 49
Birunthan 2:59eeb8637e5d 50 acc.readAccel(); // On lit de nouveau les valeurs sur l'accéléromètre et on les compare avec les valeurs précédentes
Birunthan 2:59eeb8637e5d 51 epsilonx = acc.ax-axx0; // écart selon x entre la nouvelle accélération et l'ancienne accélértion
Birunthan 2:59eeb8637e5d 52 epsilony = acc.ay-ayy0; // écart selon y entre la nouvelle accélération et l'ancienne accélértion
MaxLaMenace 0:a2a605398a75 53
MaxLaMenace 0:a2a605398a75 54 while (1) {
MaxLaMenace 0:a2a605398a75 55
MaxLaMenace 0:a2a605398a75 56 // Calibration nécessaire à vitesse nul (position immobile), l'accéleration est nulle
MaxLaMenace 0:a2a605398a75 57 // On calcule l'écart entre la valeur lue et la valeur initial (contenant le bruit)
MaxLaMenace 0:a2a605398a75 58 // On veut que l'accéleration qu'on mesure soit nul, pour cela, nous devons trouver axx0
MaxLaMenace 0:a2a605398a75 59 // le plus précisement possible ( à 10^-2). On pourra alors retrancher axx0 au futur valeur.
Birunthan 2:59eeb8637e5d 60 // NB : D'autres méthodes sont possibles, par exemple faire la moyenne des n premières valeurs...
Birunthan 2:59eeb8637e5d 61 while(abs(epsilonx)>0.001 or abs(epsilony)>0.001)
MaxLaMenace 0:a2a605398a75 62 {
MaxLaMenace 0:a2a605398a75 63 axx0=axx0+epsilonx;
MaxLaMenace 0:a2a605398a75 64 ayy0=ayy0+epsilony;
MaxLaMenace 0:a2a605398a75 65 acc.readAccel();
MaxLaMenace 0:a2a605398a75 66 epsilonx = acc.ax-axx0;
MaxLaMenace 0:a2a605398a75 67 epsilony = acc.ay-ayy0;
Birunthan 2:59eeb8637e5d 68 printf("epsilonx vaut : %f\n", epsilonx);
MaxLaMenace 0:a2a605398a75 69 printf("epsilony vaut : %f\n", epsilony);
MaxLaMenace 0:a2a605398a75 70 }
MaxLaMenace 0:a2a605398a75 71
MaxLaMenace 0:a2a605398a75 72 // Mesure de l'accélération en g (1g = 9,8 m/s^2)
MaxLaMenace 0:a2a605398a75 73 acc.readAccel();
Birunthan 2:59eeb8637e5d 74 axx1 = acc.ax-axx0; // accélération en x mesurée auquel nous avons retranché l'accélération initial en x
Birunthan 2:59eeb8637e5d 75 ayy1 = acc.ay-ayy0; // accélération en y mesurée auquel nous avons retranché l'accélération initial en y
MaxLaMenace 0:a2a605398a75 76
MaxLaMenace 0:a2a605398a75 77 // Condition de seuil pour eviter de prendre en compte le bruit
MaxLaMenace 0:a2a605398a75 78 if (abs(axx1) < 0.01)
MaxLaMenace 0:a2a605398a75 79 {
MaxLaMenace 0:a2a605398a75 80 axx1 = 0; // On considère que c'est du bruit donc on le néglige
MaxLaMenace 0:a2a605398a75 81 }
MaxLaMenace 0:a2a605398a75 82 if (abs(ayy1) < 0.01)
MaxLaMenace 0:a2a605398a75 83 {
MaxLaMenace 0:a2a605398a75 84 ayy1 = 0; // On considère que c'est du bruit donc on le néglige
MaxLaMenace 0:a2a605398a75 85 }
MaxLaMenace 0:a2a605398a75 86 // Calcul de la vitesse par intégration
MaxLaMenace 0:a2a605398a75 87 wait(0.0005);
Birunthan 2:59eeb8637e5d 88 vx = vx + axx1*9.8*0.0005*1000; // l'accéleration est lue en g, on souhaite l'obtenir en m/s^2. Lors de l'intégration, on multiplie tout simplement
Birunthan 2:59eeb8637e5d 89 vy = vy + ayy1*9.8*0.0005*1000; // par le temps d'intégration entre deux lectures.
Birunthan 2:59eeb8637e5d 90 V = sqrt(vx*vx+vy*vy); // Calcul de la norme de la vitesse
MaxLaMenace 0:a2a605398a75 91
Birunthan 2:59eeb8637e5d 92 if (abs(vx)<0.001) // On considère que si la vitesse mesurée est inférieur à 0.001 m/s résulte du bruit
Birunthan 2:59eeb8637e5d 93 { // Cette condition fixe notre sensibilité, ainsi toute vitesse inférieur à cette condition est tout simplement ignorée.
MaxLaMenace 0:a2a605398a75 94 vx=0;
MaxLaMenace 0:a2a605398a75 95 }
MaxLaMenace 0:a2a605398a75 96 if (abs(vy)<0.001)
MaxLaMenace 0:a2a605398a75 97 {
MaxLaMenace 0:a2a605398a75 98 vy=0;
MaxLaMenace 0:a2a605398a75 99 }
Birunthan 2:59eeb8637e5d 100 // Malgré tout, due au faite que l'accéleration n'est pas continue, nous n'arrivons pas à faire redescendre la vitesse à 0.
Birunthan 2:59eeb8637e5d 101 // (problème d'intégration de l'accéleration due à son échantillonnage)
Birunthan 2:59eeb8637e5d 102 // Pour combler ce soucis, nous comparons les 5 dernières valeurs et considérons que si l'accélération ET la vitesse est inférieur à une certaines valeurs
Birunthan 2:59eeb8637e5d 103 // alors, l'objet est bien immobile et on remet donc la vitesse à zéro.
MaxLaMenace 1:bcb2d1a61147 104 if (k == 5) {k=0;};
MaxLaMenace 1:bcb2d1a61147 105 tabax[k] = axx1;
MaxLaMenace 1:bcb2d1a61147 106 tabay[k] = ayy1;
MaxLaMenace 1:bcb2d1a61147 107 tabvx[k] = vx;
MaxLaMenace 1:bcb2d1a61147 108 tabvy[k] = vy;
MaxLaMenace 1:bcb2d1a61147 109 if ( tabax[0]<0.1 && tabax[1]<0.1 && tabax[2]<0.1 && tabax[3]<0.1 && tabax[4]<0.1 && tabvx[0]<0.1 && tabvx[1]<0.1 && tabvx[2]<0.1 && tabvx[3]<0.1 && tabvx[4]<0.1 ) {
MaxLaMenace 0:a2a605398a75 110 vx=0;
MaxLaMenace 0:a2a605398a75 111 printf("Vitesse en X mis a zero\n");
MaxLaMenace 0:a2a605398a75 112 }
MaxLaMenace 1:bcb2d1a61147 113 if ( tabay[0]<0.1 && tabay[1]<0.1 && tabay[2]<0.1 && tabay[3]<0.1 && tabay[4]<0.1 && tabvy[0]<0.1 && tabvy[1]<0.1 && tabvy[2]<0.1 && tabvy[3]<0.1 && tabvy[4]<0.1 ) {
MaxLaMenace 0:a2a605398a75 114 vy=0;
MaxLaMenace 0:a2a605398a75 115 printf("Vitesse en Y mis a zero\n");
MaxLaMenace 0:a2a605398a75 116 }
MaxLaMenace 0:a2a605398a75 117 k++;
MaxLaMenace 0:a2a605398a75 118
MaxLaMenace 0:a2a605398a75 119 // Affichage
MaxLaMenace 0:a2a605398a75 120 printf("acceleration en x = %f\n", axx1);
MaxLaMenace 0:a2a605398a75 121 printf("acceleration en y = %f\n", ayy1);
MaxLaMenace 0:a2a605398a75 122
MaxLaMenace 0:a2a605398a75 123 printf("vitesse en x = %f mm/s\n", vx);
MaxLaMenace 0:a2a605398a75 124 printf("vitesse en y = %f mm/s\n", vy);
MaxLaMenace 0:a2a605398a75 125
MaxLaMenace 0:a2a605398a75 126 //printf("acceleration en V = %f\n", V);
MaxLaMenace 0:a2a605398a75 127
MaxLaMenace 0:a2a605398a75 128 } // fin du while
MaxLaMenace 0:a2a605398a75 129
MaxLaMenace 0:a2a605398a75 130 } // fin du main