TheBeauGosse / Mbed 2 deprecated ALTIMU_v6

Accelero Brindan

Dependencies:   mbed LSM6DS33

main.cpp@0:a2a605398a75, 2019-04-15 (annotated)

Committer:
MaxLaMenace
Date:
Mon Apr 15 13:54:01 2019 +0000
Revision:
0:a2a605398a75
Child:
1:bcb2d1a61147
R

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaxLaMenace 0:a2a605398a75 1 /* mbed Microcontroller Library
MaxLaMenace 0:a2a605398a75 2 * Copyright (c) 2018 ARM Limited
MaxLaMenace 0:a2a605398a75 3 * SPDX-License-Identifier: Apache-2.0
MaxLaMenace 0:a2a605398a75 4 */
MaxLaMenace 0:a2a605398a75 5
MaxLaMenace 0:a2a605398a75 6 #include "LSM6DS33.h"
MaxLaMenace 0:a2a605398a75 7 #include <stdio.h>
MaxLaMenace 0:a2a605398a75 8 #include <stdlib.h>
MaxLaMenace 0:a2a605398a75 9
MaxLaMenace 0:a2a605398a75 10 LSM6DS33 acc = LSM6DS33(I2C_SDA, I2C_SCL);
MaxLaMenace 0:a2a605398a75 11 DigitalIn ComptIncr(PA_8);
MaxLaMenace 0:a2a605398a75 12
MaxLaMenace 0:a2a605398a75 13 float axx0;
MaxLaMenace 0:a2a605398a75 14 float ayy0;
MaxLaMenace 0:a2a605398a75 15
MaxLaMenace 0:a2a605398a75 16 float axx1;
MaxLaMenace 0:a2a605398a75 17 float ayy1;
MaxLaMenace 0:a2a605398a75 18
MaxLaMenace 0:a2a605398a75 19 float vx0;
MaxLaMenace 0:a2a605398a75 20 float vy0;
MaxLaMenace 0:a2a605398a75 21
MaxLaMenace 0:a2a605398a75 22 float vx;
MaxLaMenace 0:a2a605398a75 23 float vy;
MaxLaMenace 0:a2a605398a75 24 float V;
MaxLaMenace 0:a2a605398a75 25
MaxLaMenace 0:a2a605398a75 26 float epsilonx;
MaxLaMenace 0:a2a605398a75 27 float epsilony;
MaxLaMenace 0:a2a605398a75 28
MaxLaMenace 0:a2a605398a75 29 float tabax[5];
MaxLaMenace 0:a2a605398a75 30 float tabay[5];
MaxLaMenace 0:a2a605398a75 31 float tabvx[5];
MaxLaMenace 0:a2a605398a75 32 float tabvy[5];
MaxLaMenace 0:a2a605398a75 33
MaxLaMenace 0:a2a605398a75 34 int k;
MaxLaMenace 0:a2a605398a75 35
MaxLaMenace 0:a2a605398a75 36
MaxLaMenace 0:a2a605398a75 37 int a1; //Grandeurs pour déterminer si les roues de la voiture sont immobiles
MaxLaMenace 0:a2a605398a75 38 int a2; //ou non. Mesure si la valeur du compteur incrémental change
MaxLaMenace 0:a2a605398a75 39
MaxLaMenace 0:a2a605398a75 40 int main()
MaxLaMenace 0:a2a605398a75 41 {
MaxLaMenace 0:a2a605398a75 42 a2 = 0;
MaxLaMenace 0:a2a605398a75 43 k=0;
MaxLaMenace 0:a2a605398a75 44 vx = 0;
MaxLaMenace 0:a2a605398a75 45 vy = 0;
MaxLaMenace 0:a2a605398a75 46
MaxLaMenace 0:a2a605398a75 47 acc.begin();
MaxLaMenace 0:a2a605398a75 48
MaxLaMenace 0:a2a605398a75 49 acc.readAccel();
MaxLaMenace 0:a2a605398a75 50 axx0 = acc.ax;
MaxLaMenace 0:a2a605398a75 51 ayy0 = acc.ay;
MaxLaMenace 0:a2a605398a75 52
MaxLaMenace 0:a2a605398a75 53 acc.readAccel();
MaxLaMenace 0:a2a605398a75 54 epsilonx = acc.ax-axx0;
MaxLaMenace 0:a2a605398a75 55 epsilony = acc.ay-ayy0;
MaxLaMenace 0:a2a605398a75 56
MaxLaMenace 0:a2a605398a75 57 while (1) {
MaxLaMenace 0:a2a605398a75 58
MaxLaMenace 0:a2a605398a75 59 // Calibration nécessaire à vitesse nul (position immobile), l'accéleration est nulle
MaxLaMenace 0:a2a605398a75 60 // On calcule l'écart entre la valeur lue et la valeur initial (contenant le bruit)
MaxLaMenace 0:a2a605398a75 61 // On veut que l'accéleration qu'on mesure soit nul, pour cela, nous devons trouver axx0
MaxLaMenace 0:a2a605398a75 62 // le plus précisement possible ( à 10^-2). On pourra alors retrancher axx0 au futur valeur.
MaxLaMenace 0:a2a605398a75 63
MaxLaMenace 0:a2a605398a75 64 //a1 = ComptIncr.read();
MaxLaMenace 0:a2a605398a75 65
MaxLaMenace 0:a2a605398a75 66 while(abs(epsilonx)>0.001 or abs(epsilony)>0.001)
MaxLaMenace 0:a2a605398a75 67 {
MaxLaMenace 0:a2a605398a75 68 axx0=axx0+epsilonx;
MaxLaMenace 0:a2a605398a75 69 ayy0=ayy0+epsilony;
MaxLaMenace 0:a2a605398a75 70 acc.readAccel();
MaxLaMenace 0:a2a605398a75 71 epsilonx = acc.ax-axx0;
MaxLaMenace 0:a2a605398a75 72 epsilony = acc.ay-ayy0;
MaxLaMenace 0:a2a605398a75 73 printf("epsilonx vaut : %f\n", epsilonx);
MaxLaMenace 0:a2a605398a75 74 printf("epsilony vaut : %f\n", epsilony);
MaxLaMenace 0:a2a605398a75 75 }
MaxLaMenace 0:a2a605398a75 76
MaxLaMenace 0:a2a605398a75 77 // Mesure de l'accélération en g (1g = 9,8 m/s^2)
MaxLaMenace 0:a2a605398a75 78 acc.readAccel();
MaxLaMenace 0:a2a605398a75 79 axx1 = acc.ax-axx0;
MaxLaMenace 0:a2a605398a75 80 ayy1 = acc.ay-ayy0;
MaxLaMenace 0:a2a605398a75 81
MaxLaMenace 0:a2a605398a75 82 // Condition de seuil pour eviter de prendre en compte le bruit
MaxLaMenace 0:a2a605398a75 83 if (abs(axx1) < 0.01)
MaxLaMenace 0:a2a605398a75 84 {
MaxLaMenace 0:a2a605398a75 85 axx1 = 0; // On considère que c'est du bruit donc on le néglige
MaxLaMenace 0:a2a605398a75 86 }
MaxLaMenace 0:a2a605398a75 87 if (abs(ayy1) < 0.01)
MaxLaMenace 0:a2a605398a75 88 {
MaxLaMenace 0:a2a605398a75 89 ayy1 = 0; // On considère que c'est du bruit donc on le néglige
MaxLaMenace 0:a2a605398a75 90 }
MaxLaMenace 0:a2a605398a75 91 // Calcul de la vitesse par intégration
MaxLaMenace 0:a2a605398a75 92 wait(0.0005);
MaxLaMenace 0:a2a605398a75 93 vx = vx + axx1*9.8*0.0005*1000;
MaxLaMenace 0:a2a605398a75 94 vy = vy + ayy1*9.8*0.0005*1000;
MaxLaMenace 0:a2a605398a75 95 V = sqrt(vx*vx+vy*vy);
MaxLaMenace 0:a2a605398a75 96
MaxLaMenace 0:a2a605398a75 97 if (abs(vx)<0.001)
MaxLaMenace 0:a2a605398a75 98 {
MaxLaMenace 0:a2a605398a75 99 vx=0;
MaxLaMenace 0:a2a605398a75 100 }
MaxLaMenace 0:a2a605398a75 101 if (abs(vy)<0.001)
MaxLaMenace 0:a2a605398a75 102 {
MaxLaMenace 0:a2a605398a75 103 vy=0;
MaxLaMenace 0:a2a605398a75 104 }
MaxLaMenace 0:a2a605398a75 105
MaxLaMenace 0:a2a605398a75 106 if (k== 5) {k=0;};
MaxLaMenace 0:a2a605398a75 107 tabax[k]=axx1;
MaxLaMenace 0:a2a605398a75 108 tabay[k]=ayy1;
MaxLaMenace 0:a2a605398a75 109 tabvx[k]=vx;
MaxLaMenace 0:a2a605398a75 110 tabvy[k]=vy;
MaxLaMenace 0:a2a605398a75 111 if ( tabax[0]<0.1 && tabax[1]<0.1 && tabax[2]<0.1 && tabax[3]<0.1 && tabax[4]<0.1 && tabvx[0]<0.01 && tabvx[1]<0.01 && tabvx[2]<0.01 && tabvx[3]<0.01 && tabvx[4]<0.01 ) {
MaxLaMenace 0:a2a605398a75 112 vx=0;
MaxLaMenace 0:a2a605398a75 113 printf("Vitesse en X mis a zero\n");
MaxLaMenace 0:a2a605398a75 114 }
MaxLaMenace 0:a2a605398a75 115 if ( tabay[0]<0.1 && tabay[1]<0.1 && tabay[2]<0.1 && tabay[3]<0.1 && tabay[4]<0.1 && tabvy[0]<0.01 && tabvy[1]<0.01 && tabvy[2]<0.01 && tabvy[3]<0.01 && tabvy[4]<0.01 ) {
MaxLaMenace 0:a2a605398a75 116 vy=0;
MaxLaMenace 0:a2a605398a75 117 printf("Vitesse en Y mis a zero\n");
MaxLaMenace 0:a2a605398a75 118 }
MaxLaMenace 0:a2a605398a75 119 k++;
MaxLaMenace 0:a2a605398a75 120
MaxLaMenace 0:a2a605398a75 121 // Affichage
MaxLaMenace 0:a2a605398a75 122 printf("acceleration en x = %f\n", axx1);
MaxLaMenace 0:a2a605398a75 123 printf("acceleration en y = %f\n", ayy1);
MaxLaMenace 0:a2a605398a75 124
MaxLaMenace 0:a2a605398a75 125 printf("vitesse en x = %f mm/s\n", vx);
MaxLaMenace 0:a2a605398a75 126 printf("vitesse en y = %f mm/s\n", vy);
MaxLaMenace 0:a2a605398a75 127
MaxLaMenace 0:a2a605398a75 128 //printf("acceleration en V = %f\n", V);
MaxLaMenace 0:a2a605398a75 129
MaxLaMenace 0:a2a605398a75 130 //a2 = ComptIncr.read();
MaxLaMenace 0:a2a605398a75 131
MaxLaMenace 0:a2a605398a75 132 //if (a1=a2)
MaxLaMenace 0:a2a605398a75 133
MaxLaMenace 0:a2a605398a75 134 } // fin du while
MaxLaMenace 0:a2a605398a75 135
MaxLaMenace 0:a2a605398a75 136 } // fin du main