Commit cf4b546b54d99f18e800a25827492f0966ac5e47
1 parent
d4a6dcca
Ajout du programme pour le guidage du robot
Showing
1 changed file
with
202 additions
and
0 deletions
Show diff stats
@@ -0,0 +1,202 @@ | @@ -0,0 +1,202 @@ | ||
1 | +#include <SoftwareSerial.h> | ||
2 | + | ||
3 | +SoftwareSerial soft2(8,9); | ||
4 | + | ||
5 | +int pwma = 10; | ||
6 | +int pwmb = 11; | ||
7 | +int ain1 = 7; | ||
8 | +int ain2 = 6; | ||
9 | +int bin1 = 5; | ||
10 | +int bin2 = 4; | ||
11 | +int stby = 3; | ||
12 | +int TRIG = A1; | ||
13 | +int ECHO = A0; | ||
14 | +long cm; | ||
15 | +long lect_echo; | ||
16 | + int envoi=0; | ||
17 | + | ||
18 | +void setup() { | ||
19 | + Serial.begin(9600); | ||
20 | + soft2.begin(38400); | ||
21 | + pinMode(TRIG,OUTPUT); | ||
22 | +digitalWrite(TRIG, LOW); | ||
23 | +pinMode(ECHO,INPUT); | ||
24 | +} | ||
25 | + | ||
26 | +void loop() { | ||
27 | + String msg; | ||
28 | + char inByte='a'; | ||
29 | + int i = 0; | ||
30 | + int nb_if = 0; | ||
31 | + char id_sauv; | ||
32 | + int test_if; | ||
33 | + int nb_boucles[6]; | ||
34 | + char id_boucles[6]; | ||
35 | + int boucle_rec = 0; | ||
36 | + int capteur; | ||
37 | + //boucle dans une boucle gérée | ||
38 | + | ||
39 | + /* soft2.listen(); | ||
40 | + while(soft2.available()>0 && inByte !='\n'){ | ||
41 | + inByte=soft2.read(); | ||
42 | + msg+=inByte; | ||
43 | + delay(10); | ||
44 | + i++; | ||
45 | + envoi=1; | ||
46 | + }*/ | ||
47 | + if(envoi==0){ | ||
48 | + Serial.println("etape 0"); | ||
49 | + delay(20000); | ||
50 | + msg += (char)0x10; | ||
51 | + msg += (char)0x61; | ||
52 | + msg += (char)0x0F; | ||
53 | + msg += (char)0x20; | ||
54 | + msg += (char)0x10; | ||
55 | + msg += (char)0x70; | ||
56 | + msg += (char)0x10; | ||
57 | + msg += (char)0x61; | ||
58 | + msg += (char)0x0F; | ||
59 | + msg += (char)0x40; | ||
60 | + msg += (char)0x04; | ||
61 | + msg += (char)0x30; | ||
62 | + msg += (char)0x50; | ||
63 | + msg += (char)0x70; | ||
64 | + i = 14; | ||
65 | + envoi=1; | ||
66 | + Serial.println("etape 1"); | ||
67 | + } | ||
68 | + //++a01z++a++z test if | ||
69 | + //++B 1Q+z test while (executé 32 fois) | ||
70 | + //++B 1B +Q1Q+z test while dans un while | ||
71 | + if(envoi==1){ | ||
72 | + Serial.println("étape 2"); | ||
73 | + for(int k=0;k<i;k++){ | ||
74 | + if((msg.charAt(k) & 0xF0) ==0x10){ | ||
75 | + Serial.print("tout droit\n\r"); | ||
76 | + digitalWrite(stby,HIGH); | ||
77 | + analogWrite(pwma,100); | ||
78 | + analogWrite(pwmb,100); | ||
79 | + digitalWrite(ain1,HIGH); | ||
80 | + digitalWrite(bin1,HIGH); | ||
81 | + digitalWrite(ain2,LOW); | ||
82 | + digitalWrite(bin2,LOW); | ||
83 | + delay(1000); | ||
84 | + } | ||
85 | + | ||
86 | + else if((msg.charAt(k) & 0xF0) ==0x20){ | ||
87 | + Serial.print("à gauche\n\r"); | ||
88 | + digitalWrite(stby,HIGH); | ||
89 | + analogWrite(pwma,100); | ||
90 | + analogWrite(pwmb,100); | ||
91 | + digitalWrite(ain1,HIGH); | ||
92 | + digitalWrite(bin1,LOW); | ||
93 | + digitalWrite(ain2,LOW); | ||
94 | + digitalWrite(bin2,HIGH); | ||
95 | + delay(900); | ||
96 | + } | ||
97 | + | ||
98 | + else if((msg.charAt(k) & 0xF0) ==0x30){ | ||
99 | + Serial.print("à droite\n\r"); | ||
100 | + digitalWrite(stby,HIGH); | ||
101 | + analogWrite(pwma,100); | ||
102 | + analogWrite(pwmb,100); | ||
103 | + digitalWrite(ain1,LOW); | ||
104 | + digitalWrite(bin1,HIGH); | ||
105 | + digitalWrite(ain2,HIGH); | ||
106 | + digitalWrite(bin2,LOW); | ||
107 | + delay(900); | ||
108 | + } | ||
109 | + | ||
110 | + else if((msg.charAt(k) & 0xF0) ==0x40){ | ||
111 | + Serial.print("début while\n\r"); | ||
112 | + k++; | ||
113 | + nb_boucles[boucle_rec] = (int)msg.charAt(k); | ||
114 | + id_boucles[boucle_rec] = msg.charAt(k-1); | ||
115 | + boucle_rec++; | ||
116 | + } | ||
117 | + | ||
118 | + else if((msg.charAt(k) & 0xF0) ==0x50){ | ||
119 | + nb_boucles[boucle_rec-1]--; | ||
120 | + if(nb_boucles[boucle_rec-1]!=0){ | ||
121 | + k--; | ||
122 | + while(msg.charAt(k)!= id_boucles[boucle_rec-1]){ | ||
123 | + k--; | ||
124 | + } | ||
125 | + k++; | ||
126 | + } | ||
127 | + else{ | ||
128 | + boucle_rec--; | ||
129 | + Serial.print("fin while\n\r"); | ||
130 | + } | ||
131 | + } | ||
132 | + | ||
133 | + else if((msg.charAt(k) & 0xF0) ==0x60){ | ||
134 | + Serial.print("if\n\r"); | ||
135 | + digitalWrite(TRIG, HIGH); | ||
136 | + delayMicroseconds(50); | ||
137 | + digitalWrite(TRIG, LOW); | ||
138 | + lect_echo = pulseIn(ECHO, HIGH); | ||
139 | + cm = lect_echo / 58; | ||
140 | + Serial.println(cm); | ||
141 | + delay(10); | ||
142 | + if((msg.charAt(k) & 0x01) == 0x01){ | ||
143 | + if(msg.charAt(++k) > cm){ | ||
144 | + nb_if++; | ||
145 | + test_if=nb_if; | ||
146 | + id_sauv = msg.charAt(k+1); | ||
147 | + while(((msg.charAt(k)) & 0xF0) != 0x70 && k<i){ | ||
148 | + k++; | ||
149 | + } | ||
150 | + while(test_if!=0){ | ||
151 | + if(((msg.charAt(k) & 0xF0) == 0x60)){ | ||
152 | + k++; | ||
153 | + test_if--; | ||
154 | + } | ||
155 | + else k++; | ||
156 | + } | ||
157 | + } | ||
158 | + } | ||
159 | + else if((msg.charAt(k) & 0x01) == 0x00){ | ||
160 | + if(msg.charAt(++k) < cm){ | ||
161 | + nb_if++; | ||
162 | + test_if=nb_if; | ||
163 | + id_sauv = msg.charAt(k+1); | ||
164 | + while(((msg.charAt(k)) & 0xF0) != 0x70 && k<i){ | ||
165 | + k++; | ||
166 | + } | ||
167 | + while(test_if!=0){ | ||
168 | + if(((msg.charAt(k) & 0xF0) == 0x60)){ | ||
169 | + k++; | ||
170 | + test_if--; | ||
171 | + } | ||
172 | + else k++; | ||
173 | + } | ||
174 | + } | ||
175 | + } | ||
176 | + } | ||
177 | + | ||
178 | + else if((msg.charAt(k) & 0xF0) ==0x70){ | ||
179 | + Serial.print("fin parcours\n\r"); | ||
180 | + envoi=2; | ||
181 | + digitalWrite(stby,LOW); | ||
182 | + analogWrite(pwma,0); | ||
183 | + analogWrite(pwmb,0); | ||
184 | + digitalWrite(ain1,LOW); | ||
185 | + digitalWrite(bin1,LOW); | ||
186 | + digitalWrite(ain2,LOW); | ||
187 | + digitalWrite(bin2,LOW); | ||
188 | + k=i; | ||
189 | + } | ||
190 | + | ||
191 | + } | ||
192 | + } | ||
193 | +} | ||
194 | + | ||
195 | + | ||
196 | +/*- Tout droit: 0001 | ||
197 | +- Virage à gauche: 0010 | ||
198 | +- Virage à droite: 0011 | ||
199 | +- Début while: 0100 | ||
200 | +- Fin while: 0101 | ||
201 | +- If: 0110 | ||
202 | +- Fin parcours: 0111*/ | ||
0 | \ No newline at end of file | 203 | \ No newline at end of file |