Baptiste Massardier / Mbed 2 deprecated hcsr04

HC SR04

Dependencies:   mbed HC_SR04_Ultrasonic_Library

main.cpp@3:6bee5e6345e1, 2019-05-12 (annotated)

Committer:
Batoch
Date:
Sun May 12 15:49:48 2019 +0000
Revision:
3:6bee5e6345e1
Parent:
2:4b0821fe5e20 
c fini

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Batoch 2:4b0821fe5e20 1 #include <cmath>
Batoch 0:97403de5e127 2 #include "ultrasonic.h"
Batoch 0:97403de5e127 3 #include "mbed.h"
Batoch 0:97403de5e127 4
Batoch 2:4b0821fe5e20 5 int HAUT = 3000;
Batoch 2:4b0821fe5e20 6 int DROITE = 2000;
Batoch 2:4b0821fe5e20 7
Batoch 2:4b0821fe5e20 8 int distance1[2];
Batoch 2:4b0821fe5e20 9 int distance2[2];
Batoch 0:97403de5e127 10
Batoch 1:b47bfaaa417a 11 void dist(int distance);
Batoch 1:b47bfaaa417a 12 void dist2(int distance);
Batoch 2:4b0821fe5e20 13 int mur(int x, int y, double alpha);
Batoch 2:4b0821fe5e20 14 int comparaison(int theorique, int distance1[2], int distance2[2]);
Batoch 1:b47bfaaa417a 15
Batoch 1:b47bfaaa417a 16
Batoch 1:b47bfaaa417a 17 ultrasonic mu(A0, A1, .2, 1, &dist); //Set the trigger pin to A0 and the echo pin to A1
Batoch 2:4b0821fe5e20 18 //have updates every .2 seconds and a timeout after 1
Batoch 2:4b0821fe5e20 19 //second, and call dist when the distance changes
Batoch 1:b47bfaaa417a 20 ultrasonic mu2(A3, A4, .2, 1, &dist2);
Batoch 1:b47bfaaa417a 21
Batoch 1:b47bfaaa417a 22
Batoch 0:97403de5e127 23 void dist(int distance)
Batoch 0:97403de5e127 24 {
Batoch 0:97403de5e127 25 //put code here to execute when the distance has changed
Batoch 1:b47bfaaa417a 26 mu.pauseUpdates();
Batoch 2:4b0821fe5e20 27 distance1[1] = distance1[0];
Batoch 2:4b0821fe5e20 28 distance1[0] = distance;
Batoch 1:b47bfaaa417a 29 mu2.startUpdates();
Batoch 2:4b0821fe5e20 30
Batoch 0:97403de5e127 31 }
Batoch 1:b47bfaaa417a 32
Batoch 1:b47bfaaa417a 33 void dist2(int distance)
Batoch 1:b47bfaaa417a 34 {
Batoch 1:b47bfaaa417a 35 //put code here to execute when the distance has changed
Batoch 1:b47bfaaa417a 36 mu2.pauseUpdates();
Batoch 2:4b0821fe5e20 37 distance2[1] = distance2[0];
Batoch 2:4b0821fe5e20 38 distance2[0] = distance;
Batoch 1:b47bfaaa417a 39 mu.startUpdates();
Batoch 1:b47bfaaa417a 40 }
Batoch 1:b47bfaaa417a 41
Batoch 1:b47bfaaa417a 42
Batoch 1:b47bfaaa417a 43
Batoch 0:97403de5e127 44
Batoch 0:97403de5e127 45 int main()
Batoch 0:97403de5e127 46 {
Batoch 0:97403de5e127 47 mu.startUpdates();//start measuring the distance
Batoch 1:b47bfaaa417a 48 wait_ms(100);
Batoch 1:b47bfaaa417a 49 mu2.startUpdates();
Batoch 2:4b0821fe5e20 50 int x = 1000;//par i2c
Batoch 2:4b0821fe5e20 51 int y = 1000;
Batoch 2:4b0821fe5e20 52 double alpha = 120;//angle par i2c en degre
Batoch 0:97403de5e127 53 while(1)
Batoch 0:97403de5e127 54 {
Batoch 0:97403de5e127 55 //Do something else here
Batoch 1:b47bfaaa417a 56 mu.checkDistance();
Batoch 2:4b0821fe5e20 57 mu2.checkDistance(); //call checkDistance() as much as possible, as this is where
Batoch 2:4b0821fe5e20 58 //the class checks if dist needs to be called.
Batoch 3:6bee5e6345e1 59 printf("c'est un: %d\n", comparaison(mur(x, y, alpha), distance1, distance2));
Batoch 2:4b0821fe5e20 60 }
Batoch 2:4b0821fe5e20 61 }
Batoch 2:4b0821fe5e20 62
Batoch 2:4b0821fe5e20 63
Batoch 2:4b0821fe5e20 64
Batoch 2:4b0821fe5e20 65
Batoch 2:4b0821fe5e20 66
Batoch 2:4b0821fe5e20 67 //Retourne la distance théorique par rapport au mur
Batoch 2:4b0821fe5e20 68 int mur(int x, int y, double alpha) {
Batoch 2:4b0821fe5e20 69 int tab[2];
Batoch 2:4b0821fe5e20 70 if (alpha == 90) {
Batoch 2:4b0821fe5e20 71 int ret = HAUT-y;
Batoch 2:4b0821fe5e20 72 return ret;
Batoch 2:4b0821fe5e20 73 }
Batoch 2:4b0821fe5e20 74 if (alpha == 270) {
Batoch 2:4b0821fe5e20 75 return y;
Batoch 2:4b0821fe5e20 76 }
Batoch 2:4b0821fe5e20 77 if (0<=alpha && alpha<90) {
Batoch 2:4b0821fe5e20 78 if (tan(alpha) * DROITE + (y - (DROITE - x)) * tan(alpha) > HAUT) { //mur du haut
Batoch 2:4b0821fe5e20 79 tab[0] = (HAUT / tan(alpha)) - y + DROITE - x;
Batoch 2:4b0821fe5e20 80 tab[1] = HAUT;
Batoch 2:4b0821fe5e20 81 }
Batoch 2:4b0821fe5e20 82 else { //mur droite
Batoch 2:4b0821fe5e20 83 tab[0] = DROITE;
Batoch 2:4b0821fe5e20 84 tab[1] = tan(alpha) * x + (y - DROITE + x) * tan(alpha);
Batoch 2:4b0821fe5e20 85 }
Batoch 0:97403de5e127 86 }
Batoch 2:4b0821fe5e20 87
Batoch 2:4b0821fe5e20 88 if (90<alpha && alpha<180) {
Batoch 2:4b0821fe5e20 89 double teta = 180 - alpha;
Batoch 2:4b0821fe5e20 90 if (y + tan(teta) * x > HAUT) { //mur du haut
Batoch 2:4b0821fe5e20 91 tab[0] = ((2 * y - HAUT) / tan(teta)) + x;
Batoch 2:4b0821fe5e20 92 tab[1] = HAUT;
Batoch 2:4b0821fe5e20 93 }
Batoch 2:4b0821fe5e20 94 else { //mur gauche
Batoch 2:4b0821fe5e20 95 tab[0] = 0;
Batoch 2:4b0821fe5e20 96 tab[1] = y + tan(teta) * x;
Batoch 2:4b0821fe5e20 97 }
Batoch 2:4b0821fe5e20 98 }
Batoch 2:4b0821fe5e20 99 if (180<=alpha && alpha<270) {
Batoch 2:4b0821fe5e20 100 double teta = alpha - 180;
Batoch 2:4b0821fe5e20 101 if (y - tan(teta) * x < 0) { //mur du bas
Batoch 2:4b0821fe5e20 102 tab[0] = -y / tan(teta) - x;
Batoch 2:4b0821fe5e20 103 tab[1] = 0;
Batoch 2:4b0821fe5e20 104 }
Batoch 2:4b0821fe5e20 105 else { //mur gauche
Batoch 2:4b0821fe5e20 106 tab[0] = 0;
Batoch 2:4b0821fe5e20 107 tab[1] = y - tan(teta) * x;
Batoch 2:4b0821fe5e20 108 }
Batoch 2:4b0821fe5e20 109 }
Batoch 2:4b0821fe5e20 110 if (270<alpha && alpha<360) {
Batoch 2:4b0821fe5e20 111 double teta = 360 - alpha;
Batoch 2:4b0821fe5e20 112 if (-tan(teta) * DROITE + y < 0) { //mur du bas
Batoch 2:4b0821fe5e20 113 tab[0] = 0;
Batoch 2:4b0821fe5e20 114 tab[1] = y + tan(teta) * (DROITE - x);
Batoch 2:4b0821fe5e20 115 } else { //mur droite
Batoch 2:4b0821fe5e20 116 tab[0] = DROITE;
Batoch 2:4b0821fe5e20 117 tab[1] = -tan(teta) * DROITE + y + tan(teta) * (DROITE - x);
Batoch 2:4b0821fe5e20 118 }
Batoch 2:4b0821fe5e20 119 }
Batoch 2:4b0821fe5e20 120
Batoch 2:4b0821fe5e20 121 return (int) sqrt((double) (x-tab[0])*(x-tab[0])+(y-tab[1])*(y-tab[1]));
Batoch 2:4b0821fe5e20 122 }
Batoch 2:4b0821fe5e20 123
Batoch 2:4b0821fe5e20 124
Batoch 2:4b0821fe5e20 125
Batoch 2:4b0821fe5e20 126
Batoch 2:4b0821fe5e20 127 int comparaison(int theorique, int distance1[2], int distance2[2]){ //return 0 si c'est un mur
Batoch 2:4b0821fe5e20 128 int moyenne = (distance1[0] + distance1[1] + distance2[0] + distance2[1])/4; //Moyenne des distance mesurees
Batoch 2:4b0821fe5e20 129 if(theorique*0.95<moyenne){
Batoch 2:4b0821fe5e20 130 return 0;//c est un mur
Batoch 2:4b0821fe5e20 131 }
Batoch 2:4b0821fe5e20 132 return 1;
Batoch 0:97403de5e127 133 }