Automated thermometer

1#define ENABLE 5 // Master du controle du moteur
2#define DIRA 4 //  Tourne dans un sens 
3#define DIRB 3 // Tourne dans l'autre
4
5#include <Servo.h>
6Servo  roues; // Servo des roues 
7Servo capt; // Servo des capteurs
8
9const int  trigPin = 6; // Pin Trig attache  la broche 6
10const int echoPin = 7; // Pin Echo  attache  la broche 7
11const int neutre = 90; // Les servos reviennent neutres  90 degrs
12
13int pTurnL = 20; // Petit tourne gauche 
14int pTurnR = 160;  // Petit tourne droite
15int gTurnL = 50; // Gros tourne gauche 
16int gTurnR  = 130; // Gros tourne droite
17
18
19
20void setup() {
21  
22  Serial.begin(9600);
23  roues.attach(9); // Servo des roues attach  la pin 9
24  roues.write(neutre);
25  capt.attach(10); // Servo du capteur attach  la pin 10
26  capt.write(neutre);
27  pinMode(ENABLE, OUTPUT);
28  pinMode(DIRA, OUTPUT);
29  pinMode(DIRB, OUTPUT);
30  pinMode(trigPin, OUTPUT);
31  pinMode(echoPin, INPUT);
32}
33
34void loop()  {
35  
36  long duration, distance;
37  long dureUn, distaUn;
38  long dureDeux,  distaDeux;
39  
40  digitalWrite(trigPin, LOW); // Calcule la distance entre l'avant  de la voiture et les obstacles.
41  delayMicroseconds(2);
42  digitalWrite(trigPin,  HIGH);
43  delayMicroseconds(10);
44  digitalWrite(echoPin, LOW);
45  duration  = pulseIn(echoPin, HIGH);
46  distance = (duration / 2) / 29.1;
47
48   if(distance  > 30) {
49    Serial.print(distance);
50    Serial.println(" cm, Avance"); //  Si la distance est plus grande que 30cm, la voiture avance.
51    
52    digitalWrite(ENABLE,  HIGH);
53    digitalWrite(DIRA, HIGH);
54    digitalWrite(DIRB, LOW);
55    }
56
57   if(distance < 15){
58    Serial.print(distance);
59    Serial.println(" cm,  Recule"); // Si la distance est plus petite que 15cm, la voiture recule. 
60    
61    digitalWrite(ENABLE, HIGH); // Recule durant 0,5 seconde.
62    digitalWrite(DIRA,  LOW);
63    digitalWrite(DIRB, HIGH);
64    delay(500);
65
66    digitalWrite(ENABLE,  HIGH); // Arrêt.
67    digitalWrite(DIRA, LOW);
68    digitalWrite(DIRB, LOW);
69    Serial.print(distance);
70    Serial.println(" cm, Stop");
71    
72    delay(1000);  
73    capt.write(pTurnL); // Petit servo tourne vers la gauche
74    delay(500);
75
76  digitalWrite(trigPin, LOW); // Mesure de la distance de gauche.
77  delayMicroseconds(2);
78  digitalWrite(trigPin, HIGH);
79  delayMicroseconds(10);
80  digitalWrite(echoPin,  LOW);
81  dureUn = pulseIn(echoPin, HIGH);
82  distaUn = (dureUn / 2) / 29.1;
83  Serial.print(distaUn);
84  Serial.println(" cm, Gauche");
85  
86  delay(500);
87  capt.write(pTurnR); // Petit servo tourne  droite.
88  delay(500);
89  
90  digitalWrite(trigPin, LOW); // Mesure de la distance de droite.
91  delayMicroseconds(2);
92  digitalWrite(trigPin, HIGH);
93  delayMicroseconds(10);
94  digitalWrite(echoPin,  LOW);
95  dureDeux = pulseIn(echoPin, HIGH);
96  distaDeux = (dureDeux / 2) /  29.1;
97  Serial.print(distaDeux);
98  Serial.println(" cm, Droite");
99  
100  delay(500);
101  capt.write(neutre); // Petit servo revient  90 degrs
102
103    if(distaUn < distaDeux){ // Se déroule si le ct gauche est plus petit que le  cot droit.
104      roues.write(gTurnR); // Gros servo tourne  gauche.
105      delay(500);
106      digitalWrite(ENABLE, HIGH); // Avance 0,5 seconde.
107      digitalWrite(DIRA,  HIGH);
108      digitalWrite(DIRB, LOW);
109      
110      delay(500);
111
112      digitalWrite(ENABLE, HIGH); // Arrêt
113      digitalWrite(DIRA, LOW);
114      digitalWrite(DIRB, LOW);
115
116      roues.write(neutre); // Gros servo revient  90 degrs
117      }
118    if(distaDeux < distaUn){ // Se déroule si le ct droit  est plus petit que le ct gauche. 
119          roues.write(gTurnL); // Gros servo  tourne  gauche.
120      delay(500);
121      digitalWrite(ENABLE, HIGH); // Avance  0,5 secondes.
122      digitalWrite(DIRA, HIGH);
123      digitalWrite(DIRB, LOW);
124      
125      delay(500);
126
127      digitalWrite(ENABLE, HIGH); // Arrêt
128      digitalWrite(DIRA, LOW);
129      digitalWrite(DIRB, LOW);
130
131      roues.write(neutre);  // Gros servo revient  90 degrs.
132    }
133         }
134
135         
136    if(distance  < 30 ){ // Se déroules si la distance est plus petite que 30cm et plus grande que  15cm.
137       delay(1000);
138    capt.write(pTurnL); // Petit servo tourne  gauche.
139    delay(500);
140
141     digitalWrite(trigPin, LOW); // Calcul la distance de  gauche.
142     delayMicroseconds(2);
143     digitalWrite(trigPin, HIGH);
144     delayMicroseconds(10);
145     digitalWrite(echoPin, LOW);
146     dureUn = pulseIn(echoPin, HIGH);
147     distaUn  = (dureUn / 2) / 29.1;
148     Serial.print(distaUn);
149     Serial.println("  cm, Gauche");
150  
151      delay(500);
152      capt.write(pTurnR); // Petit  servo tourne  droite.
153      delay(500);
154  
155    digitalWrite(trigPin, LOW);  // Calcul de la distance de droite. 
156    delayMicroseconds(2);
157    digitalWrite(trigPin,  HIGH);
158    delayMicroseconds(10);
159    digitalWrite(echoPin, LOW);
160   dureDeux  = pulseIn(echoPin, HIGH);
161    distaDeux = (dureDeux / 2) / 29.1;
162    Serial.print(distaDeux);
163    Serial.println(" cm, Droite");
164  
165     delay(500);
166     capt.write(neutre);  // Petit servo revient  90 degrés
167
168     if(distaUn < distaDeux){ // Se droule  si le ct gauche est plus petit que le ct droit. 
169      roues.write(gTurnR); //  Gros servo tourne  gauche.
170      delay(500);
171      digitalWrite(ENABLE, HIGH);  // Avance 0,5 seconde.
172      digitalWrite(DIRA, HIGH);
173      digitalWrite(DIRB,  LOW);
174      
175      delay(500);
176
177      digitalWrite(ENABLE, HIGH); //  Arrêt
178      digitalWrite(DIRA, LOW);
179      digitalWrite(DIRB, LOW);
180
181      roues.write(neutre); // Gros servo revient  90 degrs.
182      }
183      
184     if(distaDeux < distaUn){ // Se déroule si le ct droit est plus petit que le  ct gauche.
185          roues.write(gTurnL); //Gros servo tourne  gauche.
186      delay(500);
187      digitalWrite(ENABLE, HIGH); // Avance 0,5 seconde.
188      digitalWrite(DIRA,  HIGH);
189      digitalWrite(DIRB, LOW);
190      
191      delay(500);
192
193      digitalWrite(ENABLE, HIGH); // Arrêt
194      digitalWrite(DIRA, LOW);
195      digitalWrite(DIRB, LOW);
196
197      roues.write(neutre); // Gros servo revient  90 degrés.
198      } 
199       }
200}
201