Commit c78b2b728f7b20d2920d0df34bfc10848a611e1c
1 parent
58056f07
Mise à jour des commentaires du programme de décryptage
Showing
1 changed file
with
36 additions
and
9 deletions
Show diff stats
Programme_arduino_robot
1 | #include <SoftwareSerial.h> | 1 | #include <SoftwareSerial.h> |
2 | 2 | ||
3 | +//Liaison série pour le module bluetooth | ||
3 | SoftwareSerial soft2(8,9); | 4 | SoftwareSerial soft2(8,9); |
4 | 5 | ||
6 | +//Tous les pins nécessaires à l'utilisation du contrôleur moteur | ||
5 | int pwma = 10; | 7 | int pwma = 10; |
6 | int pwmb = 11; | 8 | int pwmb = 11; |
7 | int ain1 = 7; | 9 | int ain1 = 7; |
@@ -9,23 +11,36 @@ int ain2 = 6; | @@ -9,23 +11,36 @@ int ain2 = 6; | ||
9 | int bin1 = 5; | 11 | int bin1 = 5; |
10 | int bin2 = 4; | 12 | int bin2 = 4; |
11 | int stby = 3; | 13 | int stby = 3; |
14 | + | ||
15 | +//Pins pour le détecteur ultrasons de l'arduino | ||
12 | int TRIG = A1; | 16 | int TRIG = A1; |
13 | int ECHO = A0; | 17 | int ECHO = A0; |
18 | + | ||
19 | +//Distance mesurée par le robot | ||
14 | long cm; | 20 | long cm; |
21 | +//Valeur renvoyée par le capteur ultrasons | ||
15 | long lect_echo; | 22 | long lect_echo; |
16 | - int envoi=0; | 23 | +//Variable indiquant si le chemin à déjà été reçu ou non |
24 | +int envoi=0; | ||
25 | +//Variable pour stocker chaque octet reçu | ||
17 | char inByte='a'; | 26 | char inByte='a'; |
18 | 27 | ||
19 | void setup() { | 28 | void setup() { |
29 | + //Initialiser les liaisons série, 9600 hard pour le debug, 38400 soft2 pour le module bluetooth | ||
20 | Serial.begin(9600); | 30 | Serial.begin(9600); |
21 | soft2.begin(38400); | 31 | soft2.begin(38400); |
22 | - pinMode(TRIG,OUTPUT); | ||
23 | -digitalWrite(TRIG, LOW); | ||
24 | -pinMode(ECHO,INPUT); | 32 | + |
33 | + //Setup par défaut des pins du capteur ultrasons | ||
34 | + pinMode(TRIG,OUTPUT); | ||
35 | + digitalWrite(TRIG, LOW); | ||
36 | + pinMode(ECHO,INPUT); | ||
25 | } | 37 | } |
26 | 38 | ||
27 | void loop() { | 39 | void loop() { |
40 | + //Stocker le message reçu via bluetooth | ||
28 | String msg; | 41 | String msg; |
42 | + | ||
43 | + //Variables diverses utilisées par la suite | ||
29 | int i = 0; | 44 | int i = 0; |
30 | int nb_if = 0; | 45 | int nb_if = 0; |
31 | char id_sauv; | 46 | char id_sauv; |
@@ -34,21 +49,25 @@ void loop() { | @@ -34,21 +49,25 @@ void loop() { | ||
34 | char id_boucles[6]; | 49 | char id_boucles[6]; |
35 | int boucle_rec = 0; | 50 | int boucle_rec = 0; |
36 | int capteur; | 51 | int capteur; |
52 | + | ||
37 | //boucle dans une boucle gérée | 53 | //boucle dans une boucle gérée |
38 | 54 | ||
55 | + //écouter sur le port série du module bluetooth, jusqu'à recevoir un chemin | ||
39 | soft2.listen(); | 56 | soft2.listen(); |
40 | while(soft2.available()>0 && inByte !='\n'){ | 57 | while(soft2.available()>0 && inByte !='\n'){ |
41 | inByte=soft2.read(); | 58 | inByte=soft2.read(); |
42 | - msg+=inByte; | ||
43 | - delay(10); | ||
44 | - i++; | ||
45 | - envoi=1; | 59 | + msg+=inByte; |
60 | + delay(10); | ||
61 | + i++; | ||
62 | + envoi=1; | ||
46 | } | 63 | } |
47 | - | 64 | + |
48 | //++a01z++a++z test if | 65 | //++a01z++a++z test if |
49 | //++B 1Q+z test while (executé 32 fois) | 66 | //++B 1Q+z test while (executé 32 fois) |
50 | //++B 1B +Q1Q+z test while dans un while | 67 | //++B 1B +Q1Q+z test while dans un while |
51 | if(envoi==1){ | 68 | if(envoi==1){ |
69 | + | ||
70 | + //Aller tout droit | ||
52 | for(int k=0;k<i;k++){ | 71 | for(int k=0;k<i;k++){ |
53 | if((msg.charAt(k) & 0xF0) ==0x10){ | 72 | if((msg.charAt(k) & 0xF0) ==0x10){ |
54 | Serial.print("tout droit\n\r"); | 73 | Serial.print("tout droit\n\r"); |
@@ -62,6 +81,7 @@ void loop() { | @@ -62,6 +81,7 @@ void loop() { | ||
62 | delay(1000); | 81 | delay(1000); |
63 | } | 82 | } |
64 | 83 | ||
84 | + //Tourner à gauche | ||
65 | else if((msg.charAt(k) & 0xF0) ==0x20){ | 85 | else if((msg.charAt(k) & 0xF0) ==0x20){ |
66 | Serial.print("à gauche\n\r"); | 86 | Serial.print("à gauche\n\r"); |
67 | digitalWrite(stby,HIGH); | 87 | digitalWrite(stby,HIGH); |
@@ -74,6 +94,7 @@ void loop() { | @@ -74,6 +94,7 @@ void loop() { | ||
74 | delay(900); | 94 | delay(900); |
75 | } | 95 | } |
76 | 96 | ||
97 | + //Tourner à droite | ||
77 | else if((msg.charAt(k) & 0xF0) ==0x30){ | 98 | else if((msg.charAt(k) & 0xF0) ==0x30){ |
78 | Serial.print("à droite\n\r"); | 99 | Serial.print("à droite\n\r"); |
79 | digitalWrite(stby,HIGH); | 100 | digitalWrite(stby,HIGH); |
@@ -86,6 +107,7 @@ void loop() { | @@ -86,6 +107,7 @@ void loop() { | ||
86 | delay(900); | 107 | delay(900); |
87 | } | 108 | } |
88 | 109 | ||
110 | + //Début boucle while | ||
89 | else if((msg.charAt(k) & 0xF0) ==0x40){ | 111 | else if((msg.charAt(k) & 0xF0) ==0x40){ |
90 | Serial.print("début while\n\r"); | 112 | Serial.print("début while\n\r"); |
91 | k++; | 113 | k++; |
@@ -94,7 +116,9 @@ void loop() { | @@ -94,7 +116,9 @@ void loop() { | ||
94 | boucle_rec++; | 116 | boucle_rec++; |
95 | } | 117 | } |
96 | 118 | ||
119 | + //Fin boucle while | ||
97 | else if((msg.charAt(k) & 0xF0) ==0x50){ | 120 | else if((msg.charAt(k) & 0xF0) ==0x50){ |
121 | + //LEs deux tableaux suivants stockent les différentes id des instructions while, et permettent donc de gérer le cas d'un while dans un while | ||
98 | nb_boucles[boucle_rec-1]--; | 122 | nb_boucles[boucle_rec-1]--; |
99 | if(nb_boucles[boucle_rec-1]!=0){ | 123 | if(nb_boucles[boucle_rec-1]!=0){ |
100 | k--; | 124 | k--; |
@@ -109,7 +133,9 @@ void loop() { | @@ -109,7 +133,9 @@ void loop() { | ||
109 | } | 133 | } |
110 | } | 134 | } |
111 | 135 | ||
136 | + //IF | ||
112 | else if((msg.charAt(k) & 0xF0) ==0x60){ | 137 | else if((msg.charAt(k) & 0xF0) ==0x60){ |
138 | + //Pour le if, on compare la valeur du deuxième octet à la valeur lue par la capteur, puis soit on continue le chemin actuel, soit on saute jusqu'au prochain if ayant la même id que le if actuel. | ||
113 | Serial.print("if\n\r"); | 139 | Serial.print("if\n\r"); |
114 | digitalWrite(TRIG, HIGH); | 140 | digitalWrite(TRIG, HIGH); |
115 | delayMicroseconds(50); | 141 | delayMicroseconds(50); |
@@ -154,6 +180,7 @@ void loop() { | @@ -154,6 +180,7 @@ void loop() { | ||
154 | } | 180 | } |
155 | } | 181 | } |
156 | 182 | ||
183 | + //Fin de parcours | ||
157 | else if((msg.charAt(k) & 0xF0) ==0x70){ | 184 | else if((msg.charAt(k) & 0xF0) ==0x70){ |
158 | Serial.print("fin parcours\n\r"); | 185 | Serial.print("fin parcours\n\r"); |
159 | envoi=2; | 186 | envoi=2; |