TheBeauGosse
/
ALTIMU_v6
Accelero Brindan
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?
User | Revision | Line number | New 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 |