HC SR04
Dependencies: mbed HC_SR04_Ultrasonic_Library
main.cpp@2:4b0821fe5e20, 2019-05-12 (annotated)
- Committer:
- Batoch
- Date:
- Sun May 12 15:47:10 2019 +0000
- Revision:
- 2:4b0821fe5e20
- Parent:
- 1:b47bfaaa417a
- Child:
- 3:6bee5e6345e1
oui;
Who changed what in which revision?
User | Revision | Line number | New 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 | 2:4b0821fe5e20 | 59 | printf("c'est un: %d", 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 | } |