Diff: main.cpp

Revision:

0:a2a605398a75

Child:

1:bcb2d1a61147

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 15 13:54:01 2019 +0000
@@ -0,0 +1,136 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include "LSM6DS33.h"
+#include <stdio.h>    
+#include <stdlib.h>
+
+LSM6DS33 acc = LSM6DS33(I2C_SDA, I2C_SCL);
+DigitalIn ComptIncr(PA_8);
+
+float axx0;
+float ayy0; 
+
+float axx1;
+float ayy1; 
+
+float vx0;
+float vy0; 
+
+float vx;
+float vy;
+float V;
+
+float epsilonx;
+float epsilony;
+
+float tabax[5];
+float tabay[5]; 
+float tabvx[5];
+float tabvy[5];
+
+int k;
+
+
+int a1; //Grandeurs pour déterminer si les roues de la voiture sont immobiles 
+int a2; //ou non. Mesure si la valeur du compteur incrémental change
+
+int main()
+{
+    a2 = 0;
+    k=0;
+    vx = 0;
+    vy = 0;
+    
+    acc.begin();
+    
+    acc.readAccel();
+    axx0 = acc.ax; 
+    ayy0 = acc.ay; 
+    
+    acc.readAccel();
+    epsilonx = acc.ax-axx0;
+    epsilony = acc.ay-ayy0;
+    
+    while (1) {
+        
+    // Calibration nécessaire à vitesse nul (position immobile), l'accéleration est nulle
+    // On calcule l'écart entre la valeur lue et la valeur initial (contenant le bruit)
+    // On veut que l'accéleration qu'on mesure soit nul, pour cela, nous devons trouver axx0
+    // le plus précisement possible ( à 10^-2). On pourra alors retrancher axx0 au futur valeur.
+    
+    //a1 = ComptIncr.read();
+    
+        while(abs(epsilonx)>0.001 or abs(epsilony)>0.001)
+        {
+            axx0=axx0+epsilonx;
+            ayy0=ayy0+epsilony;
+            acc.readAccel();
+            epsilonx = acc.ax-axx0;
+            epsilony = acc.ay-ayy0;
+            printf("epsilonx vaut : %f\n", epsilonx);
+            printf("epsilony vaut : %f\n", epsilony);
+        }
+        
+    // Mesure de l'accélération en g (1g = 9,8 m/s^2) 
+    acc.readAccel();
+    axx1 = acc.ax-axx0;    
+    ayy1 = acc.ay-ayy0; 
+    
+    // Condition de seuil pour eviter de prendre en  compte le bruit
+    if (abs(axx1) < 0.01) 
+        {
+        axx1 = 0; // On considère que c'est du bruit donc on le néglige
+        }
+    if (abs(ayy1) < 0.01) 
+        {
+        ayy1 = 0; // On considère que c'est du bruit donc on le néglige
+        }
+    // Calcul de la vitesse par intégration
+    wait(0.0005);
+    vx = vx + axx1*9.8*0.0005*1000;
+    vy = vy + ayy1*9.8*0.0005*1000;
+    V = sqrt(vx*vx+vy*vy);
+    
+    if (abs(vx)<0.001)
+    {
+        vx=0;
+        }
+    if (abs(vy)<0.001)
+    {
+        vy=0;
+        } 
+        
+        if (k== 5) {k=0;}; 
+        tabax[k]=axx1;
+        tabay[k]=ayy1;
+        tabvx[k]=vx;
+        tabvy[k]=vy;
+        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 ) {
+            vx=0;
+            printf("Vitesse en X mis a zero\n");
+            }
+        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 ) {
+            vy=0;
+            printf("Vitesse en Y mis a zero\n");
+            }
+            k++;
+    
+    // Affichage 
+    printf("acceleration en x = %f\n", axx1); 
+    printf("acceleration en y = %f\n", ayy1);
+    
+    printf("vitesse en x = %f mm/s\n", vx);
+    printf("vitesse en y = %f mm/s\n", vy);
+    
+    //printf("acceleration en V = %f\n", V);
+    
+    //a2 = ComptIncr.read();
+    
+    //if (a1=a2)
+    
+    } // fin du while
+
+} // fin du main
\ No newline at end of file