HC SR04
Dependencies: mbed HC_SR04_Ultrasonic_Library
Diff: main.cpp
- Revision:
- 2:4b0821fe5e20
- Parent:
- 1:b47bfaaa417a
- Child:
- 3:6bee5e6345e1
diff -r b47bfaaa417a -r 4b0821fe5e20 main.cpp --- 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