HC SR04
Dependencies: mbed HC_SR04_Ultrasonic_Library
main.cpp
- Committer:
- Batoch
- Date:
- 2019-05-12
- Revision:
- 3:6bee5e6345e1
- Parent:
- 2:4b0821fe5e20
File content as of revision 3:6bee5e6345e1:
#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 ultrasonic mu2(A3, A4, .2, 1, &dist2); void dist(int distance) { //put code here to execute when the distance has changed mu.pauseUpdates(); 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(); distance2[1] = distance2[0]; distance2[0] = distance; mu.startUpdates(); } int main() { 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. printf("c'est un: %d\n", 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; }