Diff: main.cpp

Revision:

2:4b0821fe5e20

Parent:

1:b47bfaaa417a

Child:

3:6bee5e6345e1

--- a/main.cpp	Fri May 03 18:21:19 2019 +0000
+++ b/main.cpp	Sun May 12 15:47:10 2019 +0000
@@ -1,14 +1,22 @@
+#include <cmath>
 #include "ultrasonic.h"
 #include "mbed.h"
 
+int HAUT = 3000;
+int DROITE = 2000;
+
+int distance1[2];
+int distance2[2];
 
 void dist(int distance);
 void dist2(int distance);
+int mur(int x, int y, double alpha);
+int comparaison(int theorique, int distance1[2], int distance2[2]);
 
 
 ultrasonic mu(A0, A1, .2, 1, &dist);    //Set the trigger pin to A0 and the echo pin to A1
-                                        //have updates every .2 seconds and a timeout after 1
-                                        //second, and call dist when the distance changes
+//have updates every .2 seconds and a timeout after 1
+//second, and call dist when the distance changes
 ultrasonic mu2(A3, A4, .2, 1, &dist2);
 
 
@@ -16,15 +24,18 @@
 {
     //put code here to execute when the distance has changed
     mu.pauseUpdates();
-    printf("Distance1 %d mm\r\n", distance);
+    distance1[1] = distance1[0];
+    distance1[0] = distance;
     mu2.startUpdates();
+
 }
 
 void dist2(int distance)
 {
     //put code here to execute when the distance has changed
     mu2.pauseUpdates();
-    printf("Distance2 %d mm\r\n\n", distance);
+    distance2[1] = distance2[0];
+    distance2[0] = distance;
     mu.startUpdates();
 }
 
@@ -36,11 +47,87 @@
     mu.startUpdates();//start measuring the distance
     wait_ms(100);
     mu2.startUpdates();
+    int x = 1000;//par i2c
+    int y = 1000;
+    double alpha = 120;//angle par i2c en degre
     while(1)
     {
         //Do something else here
         mu.checkDistance();
-        mu2.checkDistance();     //call checkDistance() as much as possible, as this is where
-                                //the class checks if dist needs to be called.
+        mu2.checkDistance();    //call checkDistance() as much as possible, as this is where
+        //the class checks if dist needs to be called.
+        printf("c'est un: %d", comparaison(mur(x, y, alpha), distance1, distance2));
+    }
+}
+
+
+
+
+
+//Retourne la distance théorique par rapport au mur
+int mur(int x, int y, double alpha) {
+    int tab[2];
+    if (alpha == 90) {
+        int ret = HAUT-y;
+        return ret;
+    }
+    if (alpha == 270) {
+        return y;
+    }
+    if (0<=alpha && alpha<90) {
+        if (tan(alpha) * DROITE + (y - (DROITE - x)) * tan(alpha) > HAUT) { //mur du haut
+            tab[0] = (HAUT / tan(alpha)) - y + DROITE - x;
+            tab[1] = HAUT;
+        }
+        else { //mur droite
+            tab[0] = DROITE;
+            tab[1] = tan(alpha) * x + (y - DROITE + x) * tan(alpha);
+        }
     }
+    
+    if (90<alpha && alpha<180) {
+        double teta = 180 - alpha;
+        if (y + tan(teta) * x > HAUT) { //mur du haut
+            tab[0] = ((2 * y - HAUT) / tan(teta)) + x;
+            tab[1] = HAUT;
+        }
+        else { //mur gauche
+            tab[0] = 0;
+            tab[1] = y + tan(teta) * x;
+        }
+    }
+    if (180<=alpha && alpha<270) {
+        double teta = alpha - 180;
+        if (y - tan(teta) * x < 0) { //mur du bas
+            tab[0] = -y / tan(teta) - x;
+            tab[1] = 0;
+        }
+        else { //mur gauche
+            tab[0] = 0;
+            tab[1] = y - tan(teta) * x;
+        }
+    }
+    if (270<alpha && alpha<360) {
+        double teta = 360 - alpha;
+        if (-tan(teta) * DROITE + y < 0) { //mur du bas
+            tab[0] = 0;
+            tab[1] = y + tan(teta) * (DROITE - x);
+        } else { //mur droite
+            tab[0] = DROITE;
+            tab[1] = -tan(teta) * DROITE + y + tan(teta) * (DROITE - x);
+        }
+    }
+    
+    return (int) sqrt((double) (x-tab[0])*(x-tab[0])+(y-tab[1])*(y-tab[1]));
+    }
+
+
+
+
+int comparaison(int theorique, int distance1[2], int distance2[2]){ //return 0 si c'est un mur
+    int moyenne = (distance1[0] + distance1[1] + distance2[0] + distance2[1])/4; //Moyenne des distance mesurees
+    if(theorique*0.95<moyenne){
+        return 0;//c est un mur
+    }
+    return 1;
 }
\ No newline at end of file