Otto - riadiaci program v.3
Program umožňuje priame riadenie robota (aj cez BlueTooth). Teraz už funguje aj na pinoch 2,4 a môže sa naraz programovať aj komunikovať.
Na komunikáciu s robotom môžete použiť napr. program Putty: putty.exe.
Pripojenie na vývody Arduina:
servo | pin |
---|---|
ľavá ruka | 10 |
pravá ruka | 11 |
ľavá noha | 9 |
pravá noha | 6 |
ľavá päta | 5 |
pravá päta | 3 |
TXD BlueTooth | 2 |
RXD BlueTooth | 4 |
TRIG Ultrazvuk | 7 |
ECHO Ultrazvuk | 8 |
Sirena | 12 |
Číslovanie serva v choreografii:
servo | číslo serva v choreografii |
---|---|
ľavá ruka | 5 |
pravá ruka | 6 |
ľavá noha | 4 |
pravá noha | 3 |
ľavá päta | 2 |
pravá päta | 1 |
klaves | servo/funkcia |
---|---|
a/q | ľavá ruka |
;/p | pravá ruka |
z/x | ľavá noha |
,/. | pravá noha |
d/c | ľavá päta |
k/m | pravá päta |
1/9 | zníž/zvýš rýchlosť pohybu |
H | kalibrovať strednú polohu servomotorov
|
J | kalibrovať limity pre všetky stupne voľnosti (len pre priame riadenie) |
G | zobrazenie celej kalibrácie |
E | celú kalibráciu je možné zapísať do trvalej pamäte EEPROM (používať opatrne! - dá sa prepísať najviac 100000-krát), po zapnutí sa automaticky načíta |
minus | zvukový pozdrav |
R | ruky hore-dole |
@ | načítanie choreografie, pozri príklady a formát |
? | zobrazí načítanú choreografiu (kontrola po @) |
t | zatancuje načítanú choreografiu |
U | testuje ultrazvuk |
S | zobrazí aktuálnu polohu servomotorov (vhodné pri laden |
medzera | reset všetkých motorov do strednej polohy |
3,...,6 | zmiešaný pohyb (treba nastaviť vo funkciách kombinacia1() - kombinacia4() |
v,b,n,j,h,g,f,s,l,o,i,u,y,r,e,w | zvukové efekty 1-16 |
Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií
Download: otto3.ino
1 #include <Servo.h>
2 #include <EEPROM.h>
3 #include <avr/pgmspace.h>
4
5 #define ECHO_BT_TO_USB 1
6
7 #define US_TRIG 7
8 #define US_ECHO 8
9
10 #define BT_RX 2
11 #define BT_TX 4
12
13 #define SIRENA 12
14
15 //maximalna dlzka choreografie
16 #define CHOREO_LEN 200
17
18 // cisla pinov, kde su zapojene servo motory
19 #define PIN_SERVO_LAVA_RUKA 10
20 #define PIN_SERVO_PRAVA_RUKA 11
21 #define PIN_SERVO_LAVA_NOHA 9
22 #define PIN_SERVO_PRAVA_NOHA 6
23 #define PIN_SERVO_LAVA_PATA 5
24 #define PIN_SERVO_PRAVA_PATA 3
25
26 #define S1 3
27 #define S2 5
28 #define S3 6
29 #define S4 9
30 #define S5 10
31 #define S6 11
32 #define SIRENA 12
33
34 //maximalna dlzka choreografie
35 #define CHOREO_LEN 200
36
37 // tu su serva cislovane 1-6
38 #define SERVO_LAVA_RUKA 5
39 #define SERVO_PRAVA_RUKA 6
40 #define SERVO_LAVA_NOHA 4
41 #define SERVO_PRAVA_NOHA 3
42 #define SERVO_LAVA_PATA 2
43 #define SERVO_PRAVA_PATA 1
44
45 // ak su niektore serva naopak, je tu jednotka
46 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
47
48 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
49 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
50 // co robia jednotlive znaky (znamienko urcuje smer)
51 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA,
52 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
53 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
54 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
55 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
56 SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA };
57
58 // sem si mozno ulozit svoju kalibraciu
59 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
60 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
61
62 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
63 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
64
65 Servo s[6];
66
67 uint16_t ch_time[CHOREO_LEN];
68 uint8_t ch_servo[CHOREO_LEN];
69 uint8_t ch_val[CHOREO_LEN];
70 int ch_len;
71 uint8_t kalib[6];
72 int stav[6];
73 int krok;
74 uint8_t spomalenie;
75
76 static volatile int16_t distance;
77
78 #define FIS3 2960
79 #define G3 3136
80
81 float octave_4[] = { 2093.00, 2217.46, 2349.32, 2489.02, 2637.02, 2793.83, 2959.96, 3135.96, 3322.44, 3520.00, 3729.31, 3951.07 };
82 int16_t music_speed;
83
84 //popcorn
85 uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217};
86 const uint8_t melodia1[] PROGMEM = { 252, 50, 149, 49,
87 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
88 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 149,
89 40, 49, 99, 42, 49, 99, 43, 49, 99, 42, 49, 99, 43, 49, 99, 40, 49, 99, 42, 49, 99, 40, 49, 99, 42, 49, 99, 38, 49, 99, 40, 49, 99, 38, 49, 99, 40, 49, 99, 36, 49, 99, 40, 49, 99,
90 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 49,
91 28, 31, 35, 40, 49, 99, 38, 49, 99, 40, 49, 99, 35, 49, 99, 31, 49, 99, 35, 49, 99, 28, 49, 99, 149,
92 40, 49, 99, 42, 49, 99, 43, 49, 99, 42, 49, 99, 43, 49, 99, 40, 49, 99, 42, 49, 99, 40, 49, 99, 42, 49, 99, 38, 49, 99, 40, 49, 99, 38, 49, 99, 40, 49, 99, 42, 49, 99, 43, 49, 99,
93 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99,
94 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99, 149 ,
95 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 47, 49, 99,
96 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99,
97 49, 35, 38, 43, 47, 49, 99, 45, 49, 99, 47, 49, 99, 43, 49, 99, 38, 49, 99, 43, 49, 99, 35, 49, 99, 149 ,
98 47, 49, 99, 254, 49, 99, 255, 49, 99, 254, 49, 99, 255, 49, 99, 47, 49, 99, 254, 49, 99, 47, 49, 99, 254, 49, 99, 45, 49, 99, 47, 49, 99, 45, 49, 99, 47, 49, 99, 254, 49, 99, 255, 49, 99
99 };
100
101 //kohutik jarabi
102 const uint8_t melodia2[] PROGMEM = { 252, 150, 119, 121, 173, 174, 124, 124, 124, 123, 171, 173, 123, 123, 123, 121, 169, 171, 121, 121, 121, 123, 171, 169, 119, 119 };
103
104 // kankán
105 const uint8_t melodia3[] PROGMEM = { 252, 100,
106 251,1,184,1,32,126,149,251,1,184,1,32,126,149,251,1,184,1,32,126,251,1,184,1,32,126,251,1,184,1,32,126,251,1,184,1,32,126,
107 64,71,71,73,71,69,69,73,74,78,81,78,78,76,251,1,184,1,32,126,78,68,68,78,76,69,69,73,73,71,73,71,85,83,85,83,
108 64,71,71,73,71,69,69,73,74,78,81,78,78,76,251,1,184,1,32,126,78,68,68,78,76,69,69,73,73,71,73,71,71,69,119,
109 135,131,128,126,75,76,78,80,81,76,80,76,81,76,80,76,81,76,80,76,81,76,80,76,
110 251,2,11,3,16,19,251,1,4,3,16,19,
111 251,1,4,3,16,19,251,1,4,3,16,19,
112 251,1,4,3,16,19,251,1,4,3,16,19,
113 251,1,4,3,16,19,251,1,4,3,16,19,
114 174,88,91,90,88,143,143,93,95,90,91,138,138,88,91,90,88,86,86,85,83,81,79,78,76,
115 174,88,91,90,88,143,143,93,95,90,91,138,138,88,91,90,88,86,93,89,90,136,
116 64,71,71,73,71,69,69,73,74,78,81,78,78,76,251,1,184,1,32,126,
117 78,76,126,78,76,126,78,76,126,78,76,126,78,76,126,78,76,126,
118 78,76,78,76,78,76,78,76,
119 131,119,119,119,169 };
120
121 //labutie jazero
122 const uint8_t melodia4[] PROGMEM = {
123 252, 220, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69,
124 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
125 99, 83, 81, 80,
126 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
127 149, 128, 130, 131, 133, 85, 86, 251, 6, 32, 3, 8, 86, 135, 86, 88, 251, 6, 224, 3, 8, 88, 136, 88, 90, 251, 7, 184, 3, 8, 90, 85, 81, 80, 78,
128 130, 131, 133, 85, 86, 251, 6, 32, 3, 8, 86, 135, 86, 88, 251, 6, 224, 3, 8, 88, 136, 88, 90, 251, 7, 73, 3, 8, 86, 133, 86, 91, 251, 7, 184, 3, 8, 87, 251, 7, 184, 3, 8, 85,
129 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178,
130 99, 83, 81, 80,
131 185, 78, 80, 81, 83, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 81, 251, 5, 39, 3, 8, 78, 81, 78, 73, 81, 178 };
132
133 void setup() {
134 Serial.begin(9600);
135 init_serial(9600);
136 init_ultrasonic();
137
138 randomSeed(analogRead(1));
139 s[0].attach(PIN_SERVO_PRAVA_PATA);
140 s[1].attach(PIN_SERVO_LAVA_PATA);
141 s[2].attach(PIN_SERVO_PRAVA_NOHA);
142 s[3].attach(PIN_SERVO_LAVA_NOHA);
143 s[4].attach(PIN_SERVO_LAVA_RUKA);
144 s[5].attach(PIN_SERVO_PRAVA_RUKA);
145 precitaj_kalibraciu_z_EEPROM();
146 for (int i = 0; i < 6; i++)
147 {
148 kalib[i] = prednastavena_kalibracia[i];
149 stav[i] = kalib[i];
150 s[i].write(stav[i]);
151 }
152 ch_len = 0;
153 krok = 7;
154 spomalenie = 6;
155 ahoj();
156 ruky2();
157 delay(100);
158 serial_println("\r\n Otto DTDT");
159 }
160
161 void loop() {
162 char z = -1;
163 if (serial_available()) z = serial_read();
164 #ifdef ECHO_BT_TO_USB
165 if (Serial.available()) z = Serial.read();
166 #endif
167
168 if (z != -1)
169 {
170 if (pohyb_znakom(z)) return;
171 else if (pohyb_kombinacia(z)) return;
172 else if (vydaj_zvukovy_efekt(z)) return;
173 else if (ma_zahrat_melodiu(z)) return;
174 else if (z == '@') nacitaj_choreografiu();
175 else if (z == '?') vypis_choreografiu();
176 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
177 else if (z == '-') ahoj();
178 else if (z == ' ') reset();
179 else if (z == 'H') kalibruj();
180 else if (z == 'J') nastav_limity();
181 else if (z == 'G') vypis_kalibraciu();
182 else if (z == 'L') nacitaj_kalibraciu();
183 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
184 else if (z == 'R') ruky();
185 else if (z == '9') zvys_krok();
186 else if (z == '1') zniz_krok();
187 else if (z == '8') zvys_spomalenie();
188 else if (z == '7') zniz_spomalenie();
189 else if (z == 'U') test_ultrazvuk();
190 else if (z == 'S') zobraz_stav();
191
192 }
193 int16_t d = measure_distance();
194 if (d < 10) menu_ultrasonic_request();
195 }
196
197 const uint8_t klavesy_efektov[] PROGMEM = {'v', 'b', 'n', 'j', 'h', 'g', 'f', 's', 'l', 'o', 'i', 'u', 'y', 'r', 'e', 'w'};
198 #define POCET_KLAVES_EFEKTOV 16
199
200 uint8_t vydaj_zvukovy_efekt(char z)
201 {
202 for (int i = 0; i < POCET_KLAVES_EFEKTOV; i++)
203 if (z == pgm_read_byte_near(klavesy_efektov + i))
204 {
205 zvukovy_efekt(i + 1);
206 return 1;
207 }
208 return 0;
209 }
210
211 uint8_t ma_zahrat_melodiu(char z)
212 {
213 if (z == 9) zahraj_melodiu(1);
214 else if (z == '*') zahraj_melodiu(2);
215 else if (z == '/') zahraj_melodiu(3);
216 else if (z == '+') zahraj_melodiu(4);
217 }
218
219 void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del)
220 {
221 if (*note == 251)
222 {
223 *freq = ((*(note + 1)) << 8) + (*(note + 2));
224 uint8_t d1 = *(note + 3);
225 uint8_t d2 = *(note + 4);
226 *del = (music_speed * 16 * (long)d1) / d2;
227 }
228 else if (*note == 254)
229 {
230 *freq = FIS3;
231 *del = music_speed;
232 }
233 else if (*note == 255)
234 {
235 *freq = G3;
236 *del = music_speed;
237 }
238 else
239 {
240 uint8_t len = *note / 50;
241 *del = music_speed;
242 while (len--) *del *= 2;
243 uint8_t n = *note % 50;
244 if (n != 49)
245 {
246 uint8_t octave = (n + 5) / 12;
247 n = (n + 5 ) % 12;
248 float ffreq = octave_4[n];
249 octave = 4 - octave;
250 while (octave > 0)
251 {
252 ffreq /= 2.0;
253 octave--;
254 }
255 *freq = (uint16_t)ffreq;
256 }
257 else *freq = 0;
258 }
259 }
260
261 void otto_tone(uint8_t *note)
262 {
263 uint16_t freq, d;
264 otto_translate_tone(note, &freq, &d);
265 if (freq) tone(SIRENA, freq); else noTone(SIRENA);
266 delay(d);
267 noTone(SIRENA);
268 }
269
270 void zahraj_melodiu(uint8_t cislo)
271 {
272 uint8_t nota[5];
273 music_speed = 800 / 16;
274 for (int i = 0; i < dlzka_melodia[cislo]; i++)
275 {
276 if (serial_available() || Serial.available()) break;
277 switch(cislo){
278 case 1: nota[0] = pgm_read_byte_near(melodia1 + i); break;
279 case 2: nota[0] = pgm_read_byte_near(melodia2 + i); break;
280 case 3: nota[0] = pgm_read_byte_near(melodia3 + i); break;
281 case 4: nota[0] = pgm_read_byte_near(melodia4 + i); break;
282 }
283 if (nota[0] == 252)
284 {
285 switch(cislo){
286 case 1: music_speed = pgm_read_byte_near(melodia1 + i + 1); break;
287 case 2: music_speed = pgm_read_byte_near(melodia2 + i + 1); break;
288 case 3: music_speed = pgm_read_byte_near(melodia3 + i + 1); break;
289 case 4: music_speed = pgm_read_byte_near(melodia4 + i + 1); break;
290 }
291 i++;
292 continue;
293 }
294 if (nota[0] == 251)
295 {
296 for (int k = 1; k < 5; k++)
297 switch(cislo){
298 case 1: nota[k] = pgm_read_byte_near(melodia1 + i + k); break;
299 case 2: nota[k] = pgm_read_byte_near(melodia2 + i + k); break;
300 case 3: nota[k] = pgm_read_byte_near(melodia3 + i + k); break;
301 case 4: nota[k] = pgm_read_byte_near(melodia4 + i + k); break;
302 }
303 i += 4;
304 }
305 otto_tone(nota);
306 }
307 }
308
309 void efekt1()
310 {
311 uint16_t fre = 100;
312 for (int i = 0; i < 200; i++)
313 {
314 tone(SIRENA, fre);
315 fre += 10;
316 delay(2);
317 }
318 noTone(SIRENA);
319 }
320
321 void efekt2()
322 {
323 uint16_t fre = 2000;
324 for (int i = 0; i < 200; i++)
325 {
326 tone(SIRENA, fre);
327 fre -= 10;
328 delay(2);
329 }
330 noTone(SIRENA);
331 }
332
333 void efekt3()
334 {
335 uint16_t fre;
336 for (int i = 0; i < 200; i++)
337 {
338 fre = random(3000) + 20;
339 tone(SIRENA, fre);
340 delay(2);
341 }
342 noTone(SIRENA);
343 }
344
345 void efekt4()
346 {
347 uint16_t fre = 100;
348 for (int i = 0; i < 200; i++)
349 {
350 (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
351 fre += 10;
352 delay(2);
353 }
354 noTone(SIRENA);
355 }
356
357 void efekt5()
358 {
359 uint16_t fre = 100;
360 for (int i = 0; i < 100; i++)
361 {
362 (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
363 fre += 20;
364 delay(4);
365 }
366 noTone(SIRENA);
367 }
368
369 void efekt6()
370 {
371 uint16_t d = 12;
372 for (int i = 0; i < 20; i++)
373 {
374 tone(SIRENA, 1760, 10);
375 delay(d);
376 d += 3;
377 }
378 }
379
380 void efekt7()
381 {
382 for (int i = 0; i < 40; i++)
383 {
384 (i&1) ? noTone(SIRENA): tone(SIRENA, 1760);
385 delay(10);
386 }
387 noTone(SIRENA);
388 }
389
390 void efekt8()
391 {
392 uint16_t d = 72;
393 for (int i = 0; i < 20; i++)
394 {
395 tone(SIRENA, 1760, 10);
396 delay(d);
397 d -= 3;
398 }
399 }
400
401 void efekt9()
402 {
403 float fre = 3500;
404 while (fre > 15)
405 {
406 tone(SIRENA, (uint16_t)fre);
407 fre *= 0.97;
408 delay(2);
409 }
410 noTone(SIRENA);
411 }
412
413 void efekt10()
414 {
415 float fre = 55;
416 while (fre < 3000)
417 {
418 tone(SIRENA, (uint16_t)fre);
419 fre *= 1.03;
420 delay(2);
421 }
422 noTone(SIRENA);
423 }
424
425 void efekt11()
426 {
427 uint16_t fre = 3500;
428 uint16_t d = 42;
429 for (int i = 0; i < 20; i++)
430 {
431 tone(SIRENA, fre);
432 fre -= 165;
433 delay(d);
434 d -= 2;
435 }
436 noTone(SIRENA);
437 }
438
439 void efekt12()
440 {
441 float fre = 110;
442 uint8_t d = 42;
443 for (int i = 0; i < 20; i++)
444 {
445 tone(SIRENA, fre);
446 fre += 165;
447 delay(d);
448 d -= 2;
449 }
450 noTone(SIRENA);
451 }
452
453 void efekt13()
454 {
455 uint16_t fre = 3400;
456 uint8_t d = 200;
457 uint8_t delta = 1;
458 for (int i = 0; i < 20; i++)
459 {
460 tone(SIRENA, fre);
461 fre -= 165;
462 delay(d);
463 d -= delta;
464 delta++;
465 }
466 tone(SIRENA, 110);
467 delay(1000);
468 noTone(SIRENA);
469 }
470
471 void efekt14()
472 {
473 uint16_t fre = 880;
474 int16_t d = 20;
475 for (int i = 0; i < 20; i++)
476 {
477 tone(SIRENA, fre);
478 delay(d);
479 fre += random(50) - 25;
480 d += random(10) - 5;
481 if (d < 0) d = 1;
482 }
483 noTone(SIRENA);
484 }
485
486 void efekt15()
487 {
488 uint16_t fre = 440;
489 int16_t d = 20;
490 for (int i = 0; i < 20; i++)
491 {
492 tone(SIRENA, fre);
493 delay(d);
494 fre += random(25) - 12;
495 d += random(10) - 5;
496 if (d < 0) d = 1;
497 }
498 noTone(SIRENA);
499 }
500
501 void efekt16()
502 {
503 uint16_t fre = 1760;
504 int16_t d = 20;
505 for (int i = 0; i < 20; i++)
506 {
507 tone(SIRENA, fre);
508 delay(d);
509 fre += random(100) - 50;
510 d += random(10) - 5;
511 if (d < 0) d = 1;
512 }
513 noTone(SIRENA);
514 }
515
516 void zvukovy_efekt(uint8_t cislo)
517 {
518 switch (cislo) {
519 case 1: efekt1(); break;
520 case 2: efekt2(); break;
521 case 3: efekt3(); break;
522 case 4: efekt4(); break;
523 case 5: efekt5(); break;
524 case 6: efekt6(); break;
525 case 7: efekt7(); break;
526 case 8: efekt8(); break;
527 case 9: efekt9(); break;
528 case 10: efekt10(); break;
529 case 11: efekt11(); break;
530 case 12: efekt12(); break;
531 case 13: efekt13(); break;
532 case 14: efekt14(); break;
533 case 15: efekt15(); break;
534 case 16: efekt16(); break;
535 }
536 }
537
538 void test_ultrazvuk()
539 {
540 int i = 0;
541 while ((serial_available() == 0) && (Serial.available() == 0))
542 {
543 serial_println_num(measure_distance());
544 delay(100);
545 }
546 serial_read();
547 }
548
549 void menu_ultrasonic_request()
550 {
551 uint32_t tm = millis();
552 int d = measure_distance();
553 int count = 0;
554 int count2 = 0;
555 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
556 {
557 delay(10);
558 d = measure_distance();
559 if (d == 10000) count++;
560 else count = 0;
561 if (d >= 15) count2++;
562 else count2 = 0;
563 if (serial_available() || Serial.available()) return;
564 }
565 if (millis() - tm >= 1500)
566 ultrasonic_menu();
567 }
568
569 void ultrasonic_menu()
570 {
571 int selection = 0;
572 tone(SIRENA, 880, 200);
573
574 do {
575 int count = 0;
576 do {
577 int32_t d = measure_distance();
578 if (d == 10000) continue;
579 if (d >= 20) count++;
580 else count = 0;
581 delay(10);
582 } while (!serial_available() && !Serial.available() && (count < 20));
583
584 tone(SIRENA, 440, 200);
585 uint32_t tm = millis();
586 while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
587 if (millis() - tm >= 1500)
588 {
589 tone(SIRENA, 2000, 50);
590 menu_command(selection);
591 return;
592 }
593 selection++;
594 for (int i = 0; i < selection; i++)
595 {
596 tone(SIRENA, 1261, 50);
597 delay(250);
598 }
599 } while (!serial_available() && !Serial.available());
600 while (serial_available()) serial_read();
601 while (Serial.available()) Serial.read();
602 }
603
604 void menu_command(int cmd)
605 {
606 if (cmd == 1) vpred();
607 if (cmd == 2) zahraj_melodiu(1);
608 if (cmd == 3) melodia_jedna_druhej();
609 if (cmd == 4) ahoj();
610 if (cmd == 5) zahraj_melodiu(2);
611 if (cmd == 6) zahraj_melodiu(3);
612 if (cmd == 7) zahraj_melodiu(4);
613 serial_println_num(cmd);
614 }
615
616 void melodia_jedna_druhej()
617 {
618 for (int i = 0; i < 2; i++)
619 {
620 tone(SIRENA, 262, 200);
621 delay(200);
622 tone(SIRENA, 330, 200);
623 delay(200);
624
625 tone(SIRENA, 262, 200);
626 delay(200);
627 tone(SIRENA, 330, 200);
628 delay(200);
629
630 tone(SIRENA, 392, 400);
631 delay(400);
632
633 tone(SIRENA, 392, 400);
634 delay(400);
635 }
636 noTone(8);
637 }
638
639 // chodza pre vacsieho dreveneho robota
640 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
641 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
642 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
643 uint8_t chor_len = 16;
644
645 void vpred()
646 {
647 while (measure_distance() > 30)
648 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
649 pipni();
650 reset();
651 }
652
653 void precitaj_kalibraciu_z_EEPROM()
654 {
655 uint8_t value = EEPROM.read(1);
656 if (value != '~') return;
657 for (int i = 2; i < 8; i++)
658 prednastavena_kalibracia[i - 2] = EEPROM.read(i);
659 for (int i = 0; i < 6; i++)
660 dolny_limit[i] = EEPROM.read(i + 9);
661 for (int i = 0; i < 6; i++)
662 horny_limit[i] = EEPROM.read(i + 15);
663 }
664
665 char read_char()
666 {
667 while (!serial_available() && !Serial.available());
668 if (serial_available()) return serial_read();
669 else return Serial.read();
670 }
671
672 void zapis_kalibraciu_do_EEPROM()
673 {
674 serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
675 char odpoved = read_char();
676 serial_println_char(odpoved);
677 if (odpoved == 'Y')
678 {
679 EEPROM.write(1, '~');
680 for (int i = 2; i < 8; i++)
681 EEPROM.write(i, kalib[i - 2]);
682 for (int i = 0; i < 6; i++)
683 EEPROM.write(9 + i, dolny_limit[i]);
684 for (int i = 0; i < 6; i++)
685 EEPROM.write(15 + i, horny_limit[i]);
686 serial_println("ok");
687 }
688 }
689
690 void pipni()
691 {
692 tone(SIRENA, 1568, 50);
693 delay(100);
694 tone(SIRENA, 1357, 50);
695 }
696
697 void ruky()
698 {
699 int odloz_krok = krok;
700 delay(500);
701 krok = 90;
702 pohyb(SERVO_LAVA_RUKA);
703 pohyb(SERVO_PRAVA_RUKA);
704 delay(1000);
705 krok = 180;
706 pohyb(-SERVO_LAVA_RUKA);
707 pohyb(-SERVO_PRAVA_RUKA);
708 delay(1000);
709 krok = odloz_krok;
710 pipni();
711 }
712
713 void ruky2()
714 {
715 int odloz_krok = krok;
716 delay(500);
717 krok = 180;
718 pohyb(SERVO_LAVA_RUKA);
719 pohyb(SERVO_PRAVA_RUKA);
720 delay(1000);
721 krok = 90;
722 pohyb(-SERVO_LAVA_RUKA);
723 pohyb(-SERVO_PRAVA_RUKA);
724 delay(1000);
725 krok = odloz_krok;
726 pipni();
727 }
728
729 void ahoj()
730 {
731 tone(SIRENA, 1568, 50);
732 delay(70);
733 tone(SIRENA, 1175, 30);
734 delay(50);
735 tone(SIRENA, 880, 30);
736 delay(50);
737 tone(SIRENA, 1047, 50);
738 delay(70);
739 tone(SIRENA, 1245, 30);
740 delay(150);
741 tone(SIRENA, 1568, 50);
742 delay(100);
743 if (random(10) > 4) tone(SIRENA, 1357, 50);
744 else tone(SIRENA, 1047, 50);
745 }
746
747 void nastav_koncatinu(int8_t servo, uint8_t poloha)
748 {
749 int8_t delta;
750 int8_t srv = (servo > 0)?servo:-servo;
751 srv--;
752 poloha += kalib[srv] - 90;
753 if (poloha > 180) poloha = 180;
754 if (poloha < 0) poloha = 0;
755
756 if (stav[srv] < poloha) delta = 1;
757 else delta = -1;
758 while (stav[srv] != poloha)
759 {
760 stav[srv] +=delta;
761 s[srv].write(stav[srv]);
762 delay(spomalenie);
763 }
764 }
765
766 void zobraz_stav()
767 {
768 for (int i = 0; i < 6; i++)
769 {
770 serial_print("S"); serial_print_num(i + 1); serial_print(": "); serial_println_num(stav[i] - kalib[i] + 90);
771 }
772 serial_println("---");
773 pipni();
774 }
775
776 void pohyb(int8_t servo)
777 {
778 int8_t srv = (servo > 0)?servo:-servo;
779 srv--;
780 if (servo_invertovane[srv]) servo = -servo;
781 if (servo > 0)
782 {
783 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
784 else stav[srv] = horny_limit[srv];
785 s[srv].write(stav[srv]);
786 }
787 else if (servo < 0)
788 {
789 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
790 else stav[srv] = dolny_limit[srv];
791 s[srv].write(stav[srv]);
792 }
793 }
794
795 uint8_t pohyb_znakom(char z)
796 {
797 for (int i = 0; i < 12; i++)
798 {
799 if (z == znaky_zmien[i])
800 {
801 int8_t servo = zmeny[i];
802 pohyb(servo);
803 }
804 }
805 }
806
807 void kombinacia1()
808 {
809 pohyb(SERVO_LAVA_NOHA);
810 pohyb(-SERVO_PRAVA_PATA);
811 }
812
813 void kombinacia2()
814 {
815 pohyb(SERVO_PRAVA_NOHA);
816 pohyb(-SERVO_LAVA_PATA);
817 }
818
819 void kombinacia3()
820 {
821 pohyb(SERVO_LAVA_RUKA);
822 pohyb(SERVO_PRAVA_RUKA);
823 }
824
825 void kombinacia4()
826 {
827 pohyb(-SERVO_LAVA_RUKA);
828 pohyb(-SERVO_PRAVA_RUKA);
829 }
830
831 int pohyb_kombinacia(char z)
832 {
833 if (z == '3') kombinacia1();
834 else if (z == '4') kombinacia2();
835 else if (z == '5') kombinacia3();
836 else if (z == '6') kombinacia4();
837 else return 0;
838 return 1;
839 }
840
841 int nacitajCislo()
842 {
843 int num = 0;
844 int z;
845 do {
846 z = read_char();
847 if (z == '#') while (z != 13) z = read_char();
848 } while ((z < '0') || (z > '9'));
849 while ((z >= '0') && (z <= '9'))
850 {
851 num *= 10;
852 num += (z - '0');
853 do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
854 }
855 return num;
856 }
857
858 void nacitaj_choreografiu()
859 {
860 ch_len = 0;
861 int tm;
862 do {
863 tm = nacitajCislo();
864 ch_time[ch_len] = tm;
865 ch_servo[ch_len] = nacitajCislo();
866 ch_val[ch_len] = nacitajCislo();
867 ch_len++;
868 if (ch_len == CHOREO_LEN) break;
869 } while (tm > 0);
870 pipni();
871 }
872
873 void vypis_choreografiu()
874 {
875 for (int i = 0; i < ch_len; i++)
876 {
877 serial_print_num(ch_time[i]);
878 serial_print(" ");
879 serial_print_num(ch_servo[i]);
880 serial_print(" ");
881 serial_println_num(ch_val[i]);
882 }
883 serial_println("---");
884 pipni();
885 }
886
887 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
888 {
889 for (int i = 0; i < ch_len - 1; i++)
890 {
891 delay(ch_time[i]);
892 if (ch_servo[i] < 10) nastav_koncatinu(ch_servo[i], ch_val[i]);
893 else specialny_prikaz(ch_servo[i], ch_val[i]);
894 }
895 pipni();
896 while (serial_available()) serial_read();
897 while (Serial.available()) Serial.read();
898 }
899
900 void specialny_prikaz(uint8_t prikaz, uint8_t argument)
901 {
902 if (prikaz == 10) spomalenie = argument;
903 else if (prikaz == 11) zacni_hrat_melodiu(argument);
904 else if (prikaz == 12) zvukovy_efekt(argument);
905 }
906
907 void zacni_hrat_melodiu(uint8_t cislo)
908 {
909
910 }
911
912 void reset()
913 {
914 for (int i = 0; i < 6; i++)
915 {
916 stav[i] = kalib[i];
917 s[i].write(kalib[i]);
918 delay(300);
919 }
920 pipni();
921 }
922
923 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
924 {
925 serial_print(" (+/-/ENTER): ");
926 serial_println_num(hodnota);
927 s[servo].write(hodnota);
928 char z;
929 do {
930 z = read_char();
931 if ((z == '+') && (hodnota < 180)) hodnota++;
932 else if ((z == '-') && (hodnota > 0)) hodnota--;
933 if ((z == '+') || (z == '-'))
934 {
935 serial_print_num(hodnota); serial_print_char('\r');
936 s[servo].write(hodnota);
937 }
938 } while (z != 13);
939 return hodnota;
940 }
941
942 void kalibruj()
943 {
944 for (int i = 0; i < 6; i++)
945 {
946 serial_print_num(i);
947 kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
948 serial_print_num(i);
949 serial_print(": ");
950 serial_println_num(kalib[i]);
951 }
952 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
953 serial_println("ok");
954 pipni();
955 }
956
957 void nastav_limity()
958 {
959 for (int i = 0; i < 6; i++)
960 {
961 serial_print_num(i);
962 serial_print("dolny");
963 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
964 serial_print_num(i);
965 serial_print(" dolny: ");
966 serial_println_num(dolny_limit[i]);
967 s[i].write(kalib[i]);
968
969 serial_print_num(i);
970 serial_print("horny");
971 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
972 serial_print_num(i);
973 serial_print(" horny: ");
974 serial_println_num(horny_limit[i]);
975 s[i].write(kalib[i]);
976 }
977 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
978 serial_println("ok");
979 pipni();
980 }
981
982 void vypis_kalibraciu()
983 {
984 serial_print("stredy: ");
985 for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
986 serial_println();
987 serial_print("dolny limit: ");
988 for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
989 serial_println();
990 serial_print("horny limit: ");
991 for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
992 serial_println();
993 }
994
995 void nacitaj_kalibraciu()
996 {
997 int tm;
998 for (int i = 0; i < 6; i++)
999 kalib[i] = nacitajCislo();
1000 vypis_kalibraciu();
1001 serial_println("ok");
1002 pipni();
1003 }
1004
1005 void zvys_krok()
1006 {
1007 if (krok < 180) krok++;
1008 serial_print("krok: ");
1009 serial_println_num(krok);
1010 }
1011
1012 void zniz_krok()
1013 {
1014 if (krok > 0) krok--;
1015 serial_print("krok: ");
1016 serial_println_num(krok);
1017 }
1018
1019 void zniz_spomalenie()
1020 {
1021 if (spomalenie > 0) spomalenie--;
1022 serial_print("spomalenie: ");
1023 serial_println_num(spomalenie);
1024 }
1025
1026 void zvys_spomalenie()
1027 {
1028 if (spomalenie < 100) spomalenie++;
1029 serial_print("spomalenie: ");
1030 serial_println_num(spomalenie);
1031 }
1032
1033 // nasleduje softverova implementacia serioveho portu
1034 #define SERIAL_STATE_IDLE 0
1035 #define SERIAL_STATE_RECEIVING 1
1036 #define SERIAL_BUFFER_LENGTH 20
1037
1038 static volatile uint8_t serial_state;
1039 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
1040 static volatile uint8_t serial_buf_wp, serial_buf_rp;
1041
1042 static volatile uint8_t receiving_byte;
1043
1044 static volatile uint32_t time_startbit_noticed;
1045 static volatile uint8_t next_bit_order;
1046 static volatile uint8_t waiting_stop_bit;
1047 static uint16_t one_byte_duration;
1048 static uint16_t one_bit_duration;
1049 static uint16_t one_bit_write_duration;
1050 static uint16_t half_of_one_bit_duration;
1051
1052 void init_serial(uint32_t baud_rate)
1053 {
1054 pinMode(2, INPUT);
1055 pinMode(4, OUTPUT);
1056
1057 serial_state = SERIAL_STATE_IDLE;
1058
1059 one_byte_duration = 9500000 / baud_rate;
1060 one_bit_duration = 1000000 / baud_rate;
1061 one_bit_write_duration = one_bit_duration - 1;
1062 half_of_one_bit_duration = 500000 / baud_rate;
1063
1064 PCMSK2 |= 4; //PCINT18;
1065 PCIFR &= ~4; //PCIF2;
1066 PCICR |= 4; // PCIE2;
1067 }
1068
1069 ISR(PCINT2_vect)
1070 {
1071 uint32_t tm = micros();
1072 if (serial_state == SERIAL_STATE_IDLE)
1073 {
1074 time_startbit_noticed = tm;
1075 serial_state = SERIAL_STATE_RECEIVING;
1076 receiving_byte = 0xFF;
1077 next_bit_order = 0;
1078 }
1079 else if (tm - time_startbit_noticed > one_byte_duration)
1080 {
1081 serial_buffer[serial_buf_wp] = receiving_byte;
1082 serial_buf_wp++;
1083 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1084 time_startbit_noticed = tm;
1085 receiving_byte = 0xFF;
1086 next_bit_order = 0;
1087 }
1088 else if (PIND & 4)
1089 {
1090 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1091 while (next_bit_order < new_next_bit_order)
1092 {
1093 receiving_byte &= ~(1 << next_bit_order);
1094 next_bit_order++;
1095 }
1096 if (next_bit_order == 8)
1097 {
1098 serial_buffer[serial_buf_wp] = receiving_byte;
1099 serial_buf_wp++;
1100 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1101 serial_state = SERIAL_STATE_IDLE;
1102 }
1103 } else
1104 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1105 }
1106
1107 uint8_t serial_available()
1108 {
1109 cli();
1110 if (serial_buf_rp != serial_buf_wp)
1111 {
1112 sei();
1113 return 1;
1114 }
1115 if (serial_state == SERIAL_STATE_RECEIVING)
1116 {
1117 uint32_t tm = micros();
1118 if (tm - time_startbit_noticed > one_byte_duration)
1119 {
1120 serial_state = SERIAL_STATE_IDLE;
1121 serial_buffer[serial_buf_wp] = receiving_byte;
1122 serial_buf_wp++;
1123 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1124 sei();
1125 return 1;
1126 }
1127 }
1128 sei();
1129 return 0;
1130 }
1131
1132 int16_t serial_read()
1133 {
1134 cli();
1135 if (serial_buf_rp != serial_buf_wp)
1136 {
1137 uint8_t ch = serial_buffer[serial_buf_rp];
1138 serial_buf_rp++;
1139 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
1140 sei();
1141 return ch;
1142 }
1143
1144 if (serial_state == SERIAL_STATE_RECEIVING)
1145 {
1146 uint32_t tm = micros();
1147 if (tm - time_startbit_noticed > one_byte_duration)
1148 {
1149 uint8_t ch = receiving_byte;
1150 serial_state = SERIAL_STATE_IDLE;
1151 sei();
1152 return ch;
1153 }
1154 }
1155 sei();
1156 return -1;
1157 }
1158
1159 void serial_write(uint8_t ch)
1160 {
1161 #ifdef ECHO_BT_TO_USB
1162 Serial.print((char)ch);
1163 #endif
1164 PORTD &= ~16;
1165 delayMicroseconds(one_bit_write_duration);
1166 for (uint8_t i = 0; i < 8; i++)
1167 {
1168 if (ch & 1) PORTD |= 16;
1169 else PORTD &= ~16;
1170 ch >>= 1;
1171 delayMicroseconds(one_bit_write_duration);
1172 }
1173 PORTD |= 16;
1174 delayMicroseconds(one_bit_write_duration);
1175 delayMicroseconds(one_bit_write_duration);
1176 delayMicroseconds(one_bit_write_duration);
1177 delayMicroseconds(one_bit_write_duration);
1178 delayMicroseconds(one_bit_write_duration);
1179 }
1180
1181 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
1182 {
1183 uint16_t len;
1184 uint16_t ch;
1185 do {
1186 ch = serial_read();
1187 if (ch == 13) continue;
1188 } while (ch == -1);
1189
1190 do {
1191 if ((ch != 13) && (ch != 10) && (ch != -1))
1192 {
1193 *(ln++) = ch;
1194 max_length--;
1195 len++;
1196 }
1197 ch = serial_read();
1198 } while ((ch != 13) && max_length);
1199 *ln = 0;
1200 return len;
1201 }
1202
1203 void serial_print_num(int32_t number)
1204 {
1205 if (number < 0)
1206 {
1207 serial_write('-');
1208 number = -number;
1209 }
1210 int32_t rad = 1;
1211 while (number / rad) rad *= 10;
1212 if (number > 0) rad /= 10;
1213 while (rad)
1214 {
1215 serial_write((char)('0' + (number / rad)));
1216 number -= (number / rad) * rad;
1217 rad /= 10;
1218 }
1219 }
1220
1221 void serial_print_char(char ch)
1222 {
1223 serial_write(ch);
1224 }
1225
1226 void serial_print(const uint8_t *str)
1227 {
1228 while (*str) serial_write(*(str++));
1229 }
1230
1231 void serial_println(const uint8_t *str)
1232 {
1233 serial_print(str);
1234 serial_write(13);
1235 serial_write(10);
1236 }
1237
1238 void serial_println_num(int32_t number)
1239 {
1240 serial_print_num(number);
1241 serial_println();
1242 }
1243
1244 void serial_println_char(char ch)
1245 {
1246 serial_write(ch);
1247 serial_println();
1248 }
1249
1250 void serial_println()
1251 {
1252 serial_write(13);
1253 serial_write(10);
1254 }
1255
1256 // nasleduje citanie z utltazvukoveho senzora
1257
1258 static volatile uint32_t pulse_start;
1259 static volatile uint8_t new_distance;
1260
1261 void init_ultrasonic()
1262 {
1263 pinMode(US_ECHO, INPUT);
1264 pinMode(US_TRIG, OUTPUT);
1265
1266 PCMSK0 |= 1; //PCINT0;
1267 PCIFR &= ~1; //PCIF0;
1268 PCICR |= 1; // PCIE0;
1269 }
1270
1271 ISR(PCINT0_vect)
1272 {
1273 if (PINB & 1) pulse_start = micros();
1274 else
1275 {
1276 distance = (int16_t)((micros() - pulse_start) / 58);
1277 new_distance = 1;
1278 }
1279 }
1280
1281 void start_distance_measurement()
1282 {
1283 distance = 10000;
1284 new_distance = 0;
1285 digitalWrite(US_TRIG, HIGH);
1286 delayMicroseconds(10);
1287 digitalWrite(US_TRIG, LOW);
1288 }
1289
1290 void wait_for_distance_measurement_to_complete()
1291 {
1292 uint8_t counter = 0;
1293 while ((counter < 20) && !new_distance)
1294 {
1295 delay(1);
1296 counter++;
1297 }
1298 if (counter == 20)
1299 {
1300 pinMode(US_ECHO, OUTPUT);
1301 digitalWrite(US_ECHO, HIGH);
1302 delayMicroseconds(10);
1303 digitalWrite(US_ECHO, LOW);
1304 pinMode(US_ECHO, INPUT);
1305 delayMicroseconds(5);
1306 distance = 10000;
1307 }
1308 }
1309
1310 int16_t measure_distance()
1311 {
1312 start_distance_measurement();
1313 wait_for_distance_measurement_to_complete();
1314 return distance;
1315 }