Otto - riadiaci program v.4
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:
pin | signál |
---|---|
10 | ľavá ruka |
11 | pravá ruka |
9 | ľavá noha |
6 | pravá noha |
5 | ľavá päta |
3 | pravá päta |
2 | TXD BlueTooth |
4 | RXD BlueTooth |
7 | TRIG Ultrazvuk |
8 | ECHO Ultrazvuk |
12 | Siréna + |
G | Siréna -, GND Ultrazvuk, GND BlueTooth, kondenzátor -, najtmavší káblik každého serva, oba kábliky z vypínača |
V | VCC Ultrazvuk, VCC BlueTooth, kondenzátor +, červený káblik každého serva, oba kabliky z diody |
Čí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, treba potvrdiť veľkým Y |
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í choreografie) |
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 |
TAB,*,/,+ | zahrá melódiu 1-4 |
W | zapísať choreografiu do EEPROM (opatrne) |
C | načítať choreografiu z EEPROM |
X | formátovať EEPROM na zápis choreografií (opatrne) |
V | vypni/zapni režim nudy |
A | automatický štart choreografie po zapnutí |
Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií
Download: otto4.ino
1 #include <Servo.h>
2 #include <EEPROM.h>
3 #include <avr/pgmspace.h>
4
5 // odkomentujte nasledujuci riadok, ak je vas robot vytlaceny na 3D tlaciarni
6 //#define ROBOT_3D_PRINTED 1
7
8 #define ECHO_BT_TO_USB 1
9
10 #define US_TRIG 7
11 #define US_ECHO 8
12
13 #define BT_RX 2
14 #define BT_TX 4
15
16 #define SIRENA 12
17
18 //maximalna dlzka choreografie
19 #define CHOREO_LEN 200
20
21 #define CAS_OTTO_SA_ZACNE_NUDIT 9000
22
23 // cisla pinov, kde su zapojene servo motory
24 #define PIN_SERVO_LAVA_RUKA 10
25 #define PIN_SERVO_PRAVA_RUKA 11
26 #define PIN_SERVO_LAVA_NOHA 9
27 #define PIN_SERVO_PRAVA_NOHA 6
28 #define PIN_SERVO_LAVA_PATA 5
29 #define PIN_SERVO_PRAVA_PATA 3
30
31 // tu su serva cislovane 1-6
32 #define SERVO_LAVA_RUKA 5
33 #define SERVO_PRAVA_RUKA 6
34 #define SERVO_LAVA_NOHA 4
35 #define SERVO_PRAVA_NOHA 3
36 #define SERVO_LAVA_PATA 2
37 #define SERVO_PRAVA_PATA 1
38
39 // ak su niektore serva naopak, je tu jednotka
40 uint8_t servo_invertovane[6] = {0, 0, 0, 0, 0, 0};
41
42 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
43 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
44 // co robia jednotlive znaky (znamienko urcuje smer)
45 int8_t zmeny[] = {-SERVO_LAVA_RUKA, SERVO_LAVA_RUKA,
46 SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA,
47 -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA,
48 -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA,
49 SERVO_LAVA_PATA, -SERVO_LAVA_PATA,
50 -SERVO_PRAVA_PATA, SERVO_PRAVA_PATA
51 };
52
53 // sem si mozno ulozit svoju kalibraciu
54 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
55
56 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
57 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
58
59 Servo s[6];
60
61 uint16_t ch_time[CHOREO_LEN];
62 uint8_t ch_servo[CHOREO_LEN];
63 uint8_t ch_val[CHOREO_LEN];
64 int ch_len;
65 uint8_t kalib[6];
66 int stav[6];
67 int krok;
68 uint8_t spomalenie;
69 uint8_t auto_start;
70
71 static volatile int16_t distance;
72 static uint32_t cas_ked_naposledy_stlacil;
73 static uint8_t vypni_nudu;
74
75 void setup() {
76 Serial.begin(9600);
77 init_tone2();
78 init_serial(9600);
79 init_ultrasonic();
80
81 randomSeed(analogRead(1));
82 s[0].attach(PIN_SERVO_PRAVA_PATA);
83 s[1].attach(PIN_SERVO_LAVA_PATA);
84 s[2].attach(PIN_SERVO_PRAVA_NOHA);
85 s[3].attach(PIN_SERVO_LAVA_NOHA);
86 s[4].attach(PIN_SERVO_LAVA_RUKA);
87 s[5].attach(PIN_SERVO_PRAVA_RUKA);
88 precitaj_kalibraciu_z_EEPROM();
89 for (int i = 0; i < 6; i++)
90 {
91 kalib[i] = prednastavena_kalibracia[i];
92 stav[i] = kalib[i];
93 s[i].write(stav[i]);
94 }
95 ch_len = 0;
96 krok = 7;
97 vypni_nudu = 0;
98 spomalenie = 6;
99 ahoj();
100 ruky();
101 delay(100);
102 serial_println_flash(PSTR("\r\n Otto DTDT"));
103 if (auto_start > 0)
104 {
105 delay(7000);
106 while (serial_available()) serial_read();
107 while (Serial.available()) Serial.read();
108 precitaj_choreografiu_z_EEPROM(auto_start);
109 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
110 }
111 cas_ked_naposledy_stlacil = millis();
112 }
113
114 void loop() {
115 char z = -1;
116 if (serial_available()) z = serial_read();
117 #ifdef ECHO_BT_TO_USB
118 if (Serial.available()) z = Serial.read();
119 #endif
120
121 if (z != -1)
122 {
123 cas_ked_naposledy_stlacil = millis();
124 if (pohyb_znakom(z)) return;
125 else if (pohyb_kombinacia(z)) return;
126 else if (vydaj_zvukovy_efekt(z)) return;
127 else if (ma_zahrat_melodiu(z)) return;
128 else if (z == '@') nacitaj_choreografiu();
129 else if (z == '?') vypis_choreografiu();
130 else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
131 else if (z == '-') ahoj();
132 else if (z == ' ') reset();
133 else if (z == 'H') kalibruj();
134 else if (z == 'J') nastav_limity();
135 else if (z == 'G') vypis_kalibraciu();
136 else if (z == 'L') nacitaj_kalibraciu();
137 else if (z == 'E') zapis_kalibraciu_do_EEPROM();
138 else if (z == 'R') ruky();
139 else if (z == '9') zvys_krok();
140 else if (z == '1') zniz_krok();
141 else if (z == '8') zvys_spomalenie();
142 else if (z == '7') zniz_spomalenie();
143 else if (z == 'U') test_ultrazvuk();
144 else if (z == 'S') zobraz_stav();
145 else if (z == 'W') skus_zapisat_choreografiu_do_EEPROM();
146 else if (z == 'X') skus_naformatovat_EEPROM_choreografie();
147 else if (z == 'C') skus_nacitat_choreografiu_z_EEPROM();
148 else if (z == 'A') nastav_automaticky_start();
149 else if (z == 'V') prepni_nudu();
150 else if (z == '$') vasa_dalsia_funkcia();
151 }
152 int16_t d = measure_distance();
153 if (d < 10) menu_ultrasonic_request();
154
155 skus_ci_sa_nenudi();
156 }
157
158 void vasa_dalsia_funkcia()
159 {
160 serial_println_flash(PSTR("lalala"));
161 }
162
163 void nastav_automaticky_start()
164 {
165 serial_print_flash(PSTR("Automaticky start [1,2,3, 0=vypni]:"));
166 char z;
167 do { z = read_char(); } while ((z < '0') || (z > '3'));
168 if (z == '0') EEPROM.write(1, '~');
169 else EEPROM.write(1, z);
170 serial_println_char(z);
171 serial_println_flash(PSTR("ok"));
172 }
173
174 void prepni_nudu()
175 {
176 vypni_nudu ^= 1;
177 serial_print_flash(PSTR("ticho:"));
178 serial_println_num(vypni_nudu);
179 }
180
181 void skus_ci_sa_nenudi()
182 {
183 if ((millis() - cas_ked_naposledy_stlacil > CAS_OTTO_SA_ZACNE_NUDIT) && !vypni_nudu)
184 {
185 uint8_t rnd = random(21);
186 switch (rnd)
187 {
188 case 0: zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len); break;
189 case 1: zahraj_melodiu(1); break;
190 case 2: zahraj_melodiu(2); break;
191 case 3: zahraj_melodiu(3); break;
192 case 4: zahraj_melodiu(4); break;
193 case 5: for (int i = 0; i < 20; i++)
194 {
195 rnd = random(16) + 1;
196 zvukovy_efekt(rnd);
197 }
198 break;
199 default: zvukovy_efekt(rnd - 5); break;
200 }
201 cas_ked_naposledy_stlacil = millis();
202 }
203 }
204
205 const uint8_t klavesy_efektov[] PROGMEM = {'v', 'b', 'n', 'j', 'h', 'g', 'f', 's', 'l', 'o', 'i', 'u', 'y', 'r', 'e', 'w'};
206 #define POCET_KLAVES_EFEKTOV 16
207
208 uint8_t vydaj_zvukovy_efekt(char z)
209 {
210 for (int i = 0; i < POCET_KLAVES_EFEKTOV; i++)
211 if (z == pgm_read_byte(klavesy_efektov + i))
212 {
213 zvukovy_efekt(i + 1);
214 return 1;
215 }
216 return 0;
217 }
218
219 uint8_t ma_zahrat_melodiu(char z)
220 {
221 if (z == 9) zahraj_melodiu(1);
222 else if (z == '*') zahraj_melodiu(2);
223 else if (z == '/') zahraj_melodiu(3);
224 else if (z == '+') zahraj_melodiu(4);
225 else if (z == 27) zastav_melodiu();
226 else return 0;
227 return 1;
228 }
229
230 void efekt1()
231 {
232 uint16_t fre = 100;
233 for (int i = 0; i < 200; i++)
234 {
235 tone2(fre, 2);
236 fre += 10;
237 delay(2);
238 }
239 }
240
241 void efekt2()
242 {
243 uint16_t fre = 2060;
244 for (int i = 0; i < 200; i++)
245 {
246 tone2(fre, 2);
247 fre -= 10;
248 delay(2);
249 }
250 }
251
252 void efekt3()
253 {
254 uint16_t fre;
255 for (int i = 0; i < 200; i++)
256 {
257 fre = random(3000) + 20;
258 tone2(fre, 2);
259 delay(2);
260 }
261 }
262
263 void efekt4()
264 {
265 uint16_t fre = 100;
266 for (int i = 0; i < 200; i++)
267 {
268 (i & 1) ? tone2(fre, 2) : tone2(2100 - fre, 2);
269 fre += 10;
270 delay(2);
271 }
272 }
273
274 void efekt5()
275 {
276 uint16_t fre = 100;
277 for (int i = 0; i < 100; i++)
278 {
279 (i & 1) ? tone2(fre, 4) : tone2(2100 - fre, 4);
280 fre += 20;
281 delay(4);
282 }
283 }
284
285 void efekt6()
286 {
287 uint16_t d = 12;
288 for (int i = 0; i < 20; i++)
289 {
290 tone2(1760, 10);
291 delay(d);
292 d += 3;
293 }
294 }
295
296 void efekt7()
297 {
298 for (int i = 0; i < 40; i++)
299 {
300 if (i & 1) tone2(1760, 10);
301 delay(10);
302 }
303 }
304
305 void efekt8()
306 {
307 uint16_t d = 72;
308 for (int i = 0; i < 20; i++)
309 {
310 tone2(1760, 10);
311 delay(d);
312 d -= 3;
313 }
314 }
315
316 void efekt9()
317 {
318 float fre = 3500;
319 while (fre > 30)
320 {
321 tone2((uint16_t)fre, 2);
322 fre *= 0.97;
323 delay(2);
324 }
325 }
326
327 void efekt10()
328 {
329 float fre = 30;
330 while (fre < 3000)
331 {
332 tone2((uint16_t)fre, 2);
333 fre *= 1.03;
334 delay(2);
335 }
336 }
337
338 void efekt11()
339 {
340 uint16_t fre = 3500;
341 uint16_t d = 42;
342 for (int i = 0; i < 20; i++)
343 {
344 tone2(fre, d);
345 fre -= 165;
346 delay(d);
347 d -= 2;
348 }
349 }
350
351 void efekt12()
352 {
353 float fre = 110;
354 uint8_t d = 42;
355 for (int i = 0; i < 20; i++)
356 {
357 tone2(fre, d);
358 fre += 165;
359 delay(d);
360 d -= 2;
361 }
362 }
363
364 void efekt13()
365 {
366 uint16_t fre = 3400;
367 uint8_t d = 200;
368 uint8_t delta = 1;
369 for (int i = 0; i < 20; i++)
370 {
371 tone2(fre, d);
372 fre -= 165;
373 delay(d);
374 d -= delta;
375 delta++;
376 }
377 tone2(110, 1000);
378 delay(1000);
379 }
380
381 void efekt14()
382 {
383 uint16_t fre = 880;
384 int16_t d = 20;
385 for (int i = 0; i < 20; i++)
386 {
387 tone2(fre, d);
388 delay(d);
389 fre += random(50) - 25;
390 d += random(10) - 5;
391 if (d < 0) d = 1;
392 }
393 }
394
395 void efekt15()
396 {
397 uint16_t fre = 440;
398 int16_t d = 20;
399 for (int i = 0; i < 20; i++)
400 {
401 tone2(fre, d);
402 delay(d);
403 fre += random(25) - 12;
404 d += random(10) - 5;
405 if (d < 0) d = 1;
406 }
407 }
408
409 void efekt16()
410 {
411 uint16_t fre = 1760;
412 int16_t d = 20;
413 for (int i = 0; i < 20; i++)
414 {
415 tone2(fre, d);
416 delay(d);
417 fre += random(100) - 50;
418 d += random(10) - 5;
419 if (d < 0) d = 1;
420 }
421 }
422
423 void zvukovy_efekt(uint8_t cislo)
424 {
425 switch (cislo) {
426 case 1: efekt1(); break;
427 case 2: efekt2(); break;
428 case 3: efekt3(); break;
429 case 4: efekt4(); break;
430 case 5: efekt5(); break;
431 case 6: efekt6(); break;
432 case 7: efekt7(); break;
433 case 8: efekt8(); break;
434 case 9: efekt9(); break;
435 case 10: efekt10(); break;
436 case 11: efekt11(); break;
437 case 12: efekt12(); break;
438 case 13: efekt13(); break;
439 case 14: efekt14(); break;
440 case 15: efekt15(); break;
441 case 16: efekt16(); break;
442 }
443 }
444
445 void test_ultrazvuk()
446 {
447 while ((serial_available() == 0) && (Serial.available() == 0))
448 {
449 serial_println_num(measure_distance());
450 delay(100);
451 }
452 serial_read();
453 }
454
455 void menu_ultrasonic_request()
456 {
457 uint32_t tm = millis();
458 int d = measure_distance();
459 int count = 0;
460 int count2 = 0;
461 while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10))
462 {
463 delay(10);
464 d = measure_distance();
465 if (d == 10000) count++;
466 else count = 0;
467 if (d >= 15) count2++;
468 else count2 = 0;
469 if (serial_available() || Serial.available()) return;
470 }
471 if (millis() - tm >= 1500)
472 {
473 ultrasonic_menu();
474 cas_ked_naposledy_stlacil = millis();
475 }
476 }
477
478 void ultrasonic_menu()
479 {
480 int selection = 0;
481 tone2( 880, 200);
482
483 do {
484 int count = 0;
485 int cnt10000 = 0;
486 do {
487 int32_t d = measure_distance();
488 if (d == 10000)
489 {
490 cnt10000++;
491 delay(10);
492 if (cnt10000 == 30) break;
493 continue;
494 }
495 if (d >= 20) count++;
496 else count = 0;
497 delay(10);
498 } while (!serial_available() && !Serial.available() && (count < 20));
499
500 tone2( 440, 200);
501 uint32_t tm = millis();
502 while ((millis() - tm < 2000) && !serial_available() && !Serial.available())
503 {
504 int32_t d = measure_distance();
505 if (d < 15) count++;
506 else count = 0;
507 if (count > 10) break;
508 delay(20);
509 }
510
511 if (millis() - tm >= 2000)
512 {
513 tone2( 2000, 50);
514 menu_command(selection);
515 return;
516 }
517 selection++;
518 for (int i = 0; i < selection; i++)
519 {
520 tone2( 1261, 50);
521 delay(250);
522 }
523 } while (!serial_available() && !Serial.available());
524 while (serial_available()) serial_read();
525 while (Serial.available()) Serial.read();
526 }
527
528 void menu_command(int cmd)
529 {
530 if (cmd == 1) vpred();
531 if (cmd == 2) {
532 precitaj_choreografiu_z_EEPROM(1);
533 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
534 }
535 if (cmd == 3) {
536 precitaj_choreografiu_z_EEPROM(2);
537 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
538 }
539 if (cmd == 4) {
540 precitaj_choreografiu_z_EEPROM(3);
541 zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
542 }
543 if (cmd == 5) zahraj_melodiu(1);
544 if (cmd == 6) melodia_jedna_druhej();
545 if (cmd == 7) ahoj();
546 if (cmd == 8) zahraj_melodiu(2);
547 if (cmd == 9) zahraj_melodiu(3);
548 if (cmd == 10) zahraj_melodiu(4);
549 }
550
551 void melodia_jedna_druhej()
552 {
553 for (int i = 0; i < 2; i++)
554 {
555 tone2(262, 200);
556 delay(200);
557 tone2(330, 200);
558 delay(200);
559
560 tone2(262, 200);
561 delay(200);
562 tone2(330, 200);
563 delay(200);
564
565 tone2(392, 400);
566 delay(400);
567
568 tone2(392, 400);
569 delay(400);
570 }
571 }
572
573 // chodza pre vacsieho dreveneho robota
574 uint16_t chor_time[] = {500, 100, 1, 1, 100, 1, 1, 1, 100, 1, 1, 1, 100, 1, 100, 1, 1, 0};
575 uint8_t chor_servo[] = {11, 1, 2, 6, 4, 3, 6, 5, 1, 2, 6, 5, 3, 4, 1, 2, 9, 0};
576 uint8_t chor_val[] = {1, 48, 69, 180, 104, 104, 90, 0, 111, 146, 0, 90, 62, 69, 48, 69, 2, 0};
577 uint8_t chor_len = 18;
578
579 void vpred()
580 {
581 while ((measure_distance() > 30) && !serial_available() && !Serial.available())
582 zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
583 pipni();
584 reset();
585 }
586
587 void pipni()
588 {
589 tone2( 1568, 50);
590 delay(100);
591 tone2( 1357, 50);
592 }
593
594 void ruky()
595 {
596 uint8_t odloz_spomalenie = spomalenie;
597 spomalenie = 0;
598 #ifdef ROBOT_3D_PRINTED
599 nastav_koncatinu(5, 180);
600 nastav_koncatinu(6, 180);
601 #else
602 nastav_koncatinu(5, 0);
603 nastav_koncatinu(6, 0);
604 #endif
605 delay(1000);
606 nastav_koncatinu(5, 90);
607 nastav_koncatinu(6, 90);
608 spomalenie = odloz_spomalenie;
609 delay(1000);
610 pipni();
611 }
612
613 void ahoj()
614 {
615 tone2( 1568, 50);
616 delay(70);
617 tone2( 1175, 30);
618 delay(50);
619 tone2( 880, 30);
620 delay(50);
621 tone2( 1047, 50);
622 delay(70);
623 tone2( 1245, 30);
624 delay(150);
625 tone2( 1568, 50);
626 delay(100);
627 if (random(10) > 4) tone2( 1357, 50);
628 else tone2( 1047, 50);
629 }
630
631 void nastav_koncatinu(int8_t srv, uint8_t poloha)
632 {
633 int8_t delta;
634 srv--;
635
636 if (servo_invertovane[srv]) poloha = 180 - poloha;
637
638 if ((int16_t)poloha + (int16_t)kalib[srv] - 90 < 0) poloha = 0;
639 else poloha += kalib[srv] - 90;
640
641 if (poloha > 180) poloha = 180;
642
643 if (spomalenie == 0) {
644 stav[srv] = poloha;
645 s[srv].write(stav[srv]);
646 }
647 else {
648 if (stav[srv] < poloha) delta = 1;
649 else delta = -1;
650 while (stav[srv] != poloha)
651 {
652 stav[srv] += delta;
653 s[srv].write(stav[srv]);
654 delay(spomalenie);
655 }
656 }
657 }
658
659 void zobraz_stav()
660 {
661 for (int i = 0; i < 6; i++)
662 {
663 serial_print_flash(PSTR("S")); serial_print_num(i + 1); serial_print_flash(PSTR(": ")); serial_println_num(stav[i] - kalib[i] + 90);
664 }
665 serial_println_flash(PSTR("---"));
666 pipni();
667 }
668
669 void pohyb(int8_t servo)
670 {
671 int8_t srv = (servo > 0) ? servo : -servo;
672 srv--;
673 if (servo_invertovane[srv]) servo = -servo;
674 if (servo > 0)
675 {
676 if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
677 else stav[srv] = horny_limit[srv];
678 s[srv].write(stav[srv]);
679 }
680 else if (servo < 0)
681 {
682 if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
683 else stav[srv] = dolny_limit[srv];
684 s[srv].write(stav[srv]);
685 }
686 }
687
688 uint8_t pohyb_znakom(char z)
689 {
690 for (int i = 0; i < 12; i++)
691 {
692 if (z == znaky_zmien[i])
693 {
694 int8_t servo = zmeny[i];
695 pohyb(servo);
696 return 1;
697 }
698 }
699 return 0;
700 }
701
702 void kombinacia1()
703 {
704 pohyb(SERVO_LAVA_NOHA);
705 pohyb(-SERVO_PRAVA_PATA);
706 }
707
708 void kombinacia2()
709 {
710 pohyb(SERVO_PRAVA_NOHA);
711 pohyb(-SERVO_LAVA_PATA);
712 }
713
714 void kombinacia3()
715 {
716 pohyb(SERVO_LAVA_RUKA);
717 pohyb(SERVO_PRAVA_RUKA);
718 }
719
720 void kombinacia4()
721 {
722 pohyb(-SERVO_LAVA_RUKA);
723 pohyb(-SERVO_PRAVA_RUKA);
724 }
725
726 int pohyb_kombinacia(char z)
727 {
728 if (z == '3') kombinacia1();
729 else if (z == '4') kombinacia2();
730 else if (z == '5') kombinacia3();
731 else if (z == '6') kombinacia4();
732 else return 0;
733 return 1;
734 }
735
736 char read_char()
737 {
738 while (!serial_available() && !Serial.available());
739 if (serial_available()) return serial_read();
740 else return Serial.read();
741 }
742
743 int nacitajCislo()
744 {
745 int num = 0;
746 int z;
747 do {
748 z = read_char();
749 if (z == '#') while (z != 13) z = read_char();
750 } while ((z < '0') || (z > '9'));
751 while ((z >= '0') && (z <= '9'))
752 {
753 num *= 10;
754 num += (z - '0');
755 do {
756 z = read_char();
757 if (z == -1) delayMicroseconds(10);
758 } while (z < 0);
759 }
760 return num;
761 }
762
763 void nacitaj_choreografiu()
764 {
765 ch_len = 0;
766 int tm;
767 do {
768 tm = nacitajCislo();
769 ch_time[ch_len] = tm;
770 ch_servo[ch_len] = nacitajCislo();
771 ch_val[ch_len] = nacitajCislo();
772 ch_len++;
773 if (ch_len == CHOREO_LEN) break;
774 } while (tm > 0);
775 pipni();
776 }
777
778 void vypis_choreografiu()
779 {
780 for (int i = 0; i < ch_len; i++)
781 {
782 serial_print_num(ch_time[i]);
783 serial_print(" ");
784 serial_print_num(ch_servo[i]);
785 serial_print(" ");
786 serial_println_num(ch_val[i]);
787 }
788 serial_println_flash(PSTR("---"));
789 pipni();
790 }
791
792 uint32_t koniec_tanca;
793
794 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
795 {
796 koniec_tanca = millis() + 3600000;
797 for (int i = 0; i < ch_len - 1; i++)
798 {
799 delay(ch_time[i]);
800 if (millis() > koniec_tanca) break;
801 if (ch_servo[i] < 7) nastav_koncatinu(ch_servo[i], ch_val[i]);
802 else i = specialny_prikaz(i, ch_servo[i], ch_val[i], ch_len);
803 if (serial_available() || Serial.available()) break;
804 }
805 pipni();
806 }
807
808 int specialny_prikaz(uint16_t i, uint8_t prikaz, uint8_t argument, int len)
809 {
810 if (prikaz == 8) koniec_tanca = millis() + 1000 * (uint32_t)argument;
811 else if (prikaz == 9)
812 {
813 if (serial_available() || Serial.available()) return len - 1;
814 if (measure_distance() < 15) return len - 1;
815 return ((int)argument) - 1;
816 }
817 else if (prikaz == 10) spomalenie = argument;
818 else if (prikaz == 11) zahraj_melodiu(argument);
819 else if (prikaz == 12) zvukovy_efekt(argument);
820 return i;
821 }
822
823 void reset()
824 {
825 for (int i = 0; i < 6; i++)
826 {
827 stav[i] = kalib[i];
828 s[i].write(kalib[i]);
829 delay(300);
830 }
831 pipni();
832 }
833
834 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota, uint8_t min_hodnota, uint8_t max_hodnota)
835 {
836 serial_print_flash(PSTR(" (+/-/ENTER): "));
837 serial_println_num(hodnota);
838 s[servo].write(hodnota);
839 char z;
840 do {
841 z = read_char();
842 if ((z == '+') && (hodnota < max_hodnota)) hodnota++;
843 else if ((z == '-') && (hodnota > min_hodnota)) hodnota--;
844 if ((z == '+') || (z == '-'))
845 {
846 serial_print_num(hodnota); serial_print_char('\r');
847 s[servo].write(hodnota);
848 }
849 } while (z != 13);
850 return hodnota;
851 }
852
853 void kalibruj()
854 {
855 for (int i = 0; i < 6; i++)
856 {
857 serial_print_num(i);
858 kalib[i] = nalad_hodnotu_serva(i, kalib[i], 90-63, 90+63);
859 serial_print_num(i);
860 serial_print(": ");
861 serial_println_num(kalib[i]);
862 }
863 for (int i = 0; i < 6; i++) {
864 serial_print_num(kalib[i]);
865 serial_print(" ");
866 }
867 serial_print_flash(PSTR(" Chyt robota zozadu za telo (ENTER):"));
868 char z;
869 do { z = read_char(); } while (z != 13);
870 serial_println_char(z);
871
872 serial_print_flash(PSTR("*** Nasledujuce koncatiny su lave/prave z pohladu robota.\r\nAk to nesedi, treba prehodit kabliky servo!\r\n"));
873
874 for (int i = 0; i < 6; i++) servo_invertovane[i] = 0;
875
876 nastav_koncatinu(SERVO_PRAVA_PATA, 0);
877 serial_print_flash(PSTR("Prava pata: je von alebo dnu? (V/D):"));
878 do { z = read_char(); } while ((z != 'V') && (z != 'D'));
879 serial_println_char(z);
880 nastav_koncatinu(SERVO_PRAVA_PATA, 90);
881 if (z == 'D') servo_invertovane[SERVO_PRAVA_PATA - 1] = 0;
882 else servo_invertovane[SERVO_PRAVA_PATA - 1] = 1;
883
884 nastav_koncatinu(SERVO_LAVA_PATA, 0);
885 serial_print_flash(PSTR("Lava pata: je von alebo dnu? (V/D):"));
886 do { z = read_char(); } while ((z != 'V') && (z != 'D'));
887 serial_println_char(z);
888 nastav_koncatinu(SERVO_LAVA_PATA, 90);
889 if (z == 'V') servo_invertovane[SERVO_LAVA_PATA - 1] = 0;
890 else servo_invertovane[SERVO_LAVA_PATA - 1] = 1;
891
892 nastav_koncatinu(SERVO_PRAVA_NOHA, 0);
893 serial_print_flash(PSTR("Prava noha: vpredu je pata alebo spicka? (P/S):"));
894 do { z = read_char(); } while ((z != 'P') && (z != 'S'));
895 serial_println_char(z);
896 nastav_koncatinu(SERVO_PRAVA_NOHA, 90);
897 if (z == 'P') servo_invertovane[SERVO_PRAVA_NOHA - 1] = 1;
898 else servo_invertovane[SERVO_PRAVA_NOHA - 1] = 0;
899
900 nastav_koncatinu(SERVO_LAVA_NOHA, 0);
901 serial_print_flash(PSTR("Lava noha: vpredu je pata alebo spicka? (P/S):"));
902 do { z = read_char(); } while ((z != 'P') && (z != 'S'));
903 serial_println_char(z);
904 nastav_koncatinu(SERVO_LAVA_NOHA, 90);
905 if (z == 'S') servo_invertovane[SERVO_LAVA_NOHA - 1] = 1;
906 else servo_invertovane[SERVO_LAVA_NOHA - 1] = 0;
907
908 nastav_koncatinu(SERVO_PRAVA_RUKA, 0);
909 serial_print_flash(PSTR("Prava ruka: je hore alebo dole? (H/D):"));
910 do { z = read_char(); } while ((z != 'H') && (z != 'D'));
911 serial_println_char(z);
912 nastav_koncatinu(SERVO_PRAVA_RUKA, 90);
913 if (z == 'H') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
914 else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
915
916 nastav_koncatinu(SERVO_LAVA_RUKA, 0);
917 serial_print_flash(PSTR("Lava ruka: je hore alebo dole? (H/D):"));
918 do { z = read_char(); } while ((z != 'H') && (z != 'D'));
919 serial_println_char(z);
920 nastav_koncatinu(SERVO_LAVA_RUKA, 90);
921 if (z == 'D') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
922 else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
923
924 serial_println_flash(PSTR("ok"));
925 pipni();
926 }
927
928 void nastav_limity()
929 {
930 for (int i = 0; i < 6; i++)
931 {
932 serial_print_num(i);
933 serial_print_flash(PSTR("dolny"));
934 dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i], 0, 180);
935 serial_print_num(i);
936 serial_print_flash(PSTR(" dolny: "));
937 serial_println_num(dolny_limit[i]);
938 s[i].write(kalib[i]);
939
940 serial_print_num(i);
941 serial_print_flash(PSTR("horny"));
942 horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i], 0, 180);
943 serial_print_num(i);
944 serial_print_flash(PSTR(" horny: "));
945 serial_println_num(horny_limit[i]);
946 s[i].write(kalib[i]);
947 }
948 for (int i = 0; i < 6; i++) {
949 serial_print_num(dolny_limit[i]);
950 serial_print("-");
951 serial_print_num(horny_limit[i]);
952 serial_print(" ");
953 }
954 serial_println_flash(PSTR("ok"));
955 pipni();
956 }
957
958 void vypis_kalibraciu()
959 {
960 serial_print_flash(PSTR("stredy: "));
961 for (int i = 0; i < 6; i++) {
962 serial_print_num(kalib[i]);
963 serial_print(" ");
964 }
965 serial_println();
966
967 serial_print_flash(PSTR("invertovane servo: "));
968 for (int i = 0; i < 6; i++)
969 {
970 serial_print_num(servo_invertovane[i]);
971 serial_print_char(' ');
972 }
973 serial_println();
974
975 serial_print_flash(PSTR("dolny limit: "));
976 for (int i = 0; i < 6; i++) {
977 serial_print_num(dolny_limit[i]);
978 serial_print(" ");
979 }
980 serial_println();
981 serial_print_flash(PSTR("horny limit: "));
982 for (int i = 0; i < 6; i++) {
983 serial_print_num(horny_limit[i]);
984 serial_print(" ");
985 }
986 serial_println();
987 }
988
989 void nacitaj_kalibraciu()
990 {
991 for (int i = 0; i < 6; i++)
992 kalib[i] = nacitajCislo();
993 vypis_kalibraciu();
994 serial_println_flash(PSTR("ok"));
995 pipni();
996 }
997
998 void zvys_krok()
999 {
1000 if (krok < 180) krok++;
1001 serial_print_flash(PSTR("krok: "));
1002 serial_println_num(krok);
1003 }
1004
1005 void zniz_krok()
1006 {
1007 if (krok > 0) krok--;
1008 serial_print_flash(PSTR("krok: "));
1009 serial_println_num(krok);
1010 }
1011
1012 void zniz_spomalenie()
1013 {
1014 if (spomalenie > 0) spomalenie--;
1015 serial_print_flash(PSTR("spomalenie: "));
1016 serial_println_num(spomalenie);
1017 }
1018
1019 void zvys_spomalenie()
1020 {
1021 if (spomalenie < 100) spomalenie++;
1022 serial_print_flash(PSTR("spomalenie: "));
1023 serial_println_num(spomalenie);
1024 }
1025
1026 //nasleduju funkcie ktore pracuju s pamatom EEPROM
1027
1028 //EEPROM MAP:
1029 // 0: slot number where choreography 3 starts (B3. Real address = B3 x 4)
1030 // 1: marker '~' indicates calibration is already stored in EEPROM and will be loaded on startup
1031 // '1', '2', '3' indicate the same, but choreography 1,2, or 3 is automatically launched on startup
1032 // 2-8: servo calibration (central points to be used instead of default 90)
1033 // they are shifted by (90-63) and the range is (90-63)...0 till (90+63)...127
1034 // bit 7 (value 128) indicates servo is inverted
1035 // 9-14: lower limit for direct control for all 6 servos
1036 // 15-20: upper limit for direct control for all 6 servos
1037 // 21: number of steps in choreography 1 (L1) 0=choreography not in memory
1038 // 22: number of steps in choreography 2 (L2) 0=not in memory
1039 // 23: number of steps in choreography 3 (L3) 0=not in memory
1040 // 24..(L1 x 4 + 23) choreography1 tuplets (uint16,uint8,uint8) x L1
1041 // (1024 - L2 x 4)..1023 choreography2 same as above
1042 // B3 x 4..(B3 x 4 + L3 x 4 - 1) choreography 3 same as above
1043
1044 void skus_zapisat_choreografiu_do_EEPROM()
1045 {
1046 serial_print_flash(PSTR("Cislo? [1-3]: "));
1047 char odpoved = read_char();
1048 if ((odpoved < '1') || (odpoved > '3')) return;
1049 serial_println_char(odpoved);
1050 uint8_t cislo = odpoved - '0';
1051
1052 serial_print_flash(PSTR("Zapisat choreografiu do EEPROM c."));
1053 serial_print_char(odpoved);
1054 serial_print_flash(PSTR("? [Y/n]: "));
1055 odpoved = read_char();
1056 serial_println_char(odpoved);
1057 if (odpoved == 'Y')
1058 zapis_choreografiu_do_EEPROM(cislo);
1059 }
1060
1061 void skus_nacitat_choreografiu_z_EEPROM()
1062 {
1063 serial_print_flash(PSTR("Cislo? [1-3]: "));
1064 char odpoved = read_char();
1065 if ((odpoved < '1') || (odpoved > '3')) return;
1066 serial_println_char(odpoved);
1067 uint8_t cislo = odpoved - '0';
1068
1069 serial_print_flash(PSTR("Precitat choreografiu z EEPROM c."));
1070 serial_print_char(odpoved);
1071 serial_print_flash(PSTR("? [Y/n]: "));
1072 odpoved = read_char();
1073 serial_println_char(odpoved);
1074 if (odpoved == 'Y')
1075 precitaj_choreografiu_z_EEPROM(cislo);
1076 }
1077
1078 void skus_naformatovat_EEPROM_choreografie()
1079 {
1080 serial_print_flash(PSTR("Formatovat EEPROM choreografii? [Y/n]:"));
1081 char odpoved = read_char();
1082 serial_println_char(odpoved);
1083 if (odpoved == 'Y')
1084 naformatuj_EEPROM_choreografie();
1085 }
1086
1087 void zapis_choreografiu_do_EEPROM(int slot)
1088 {
1089 uint8_t b3 = EEPROM.read(0);
1090 uint8_t len1 = EEPROM.read(21);
1091 uint8_t len2 = EEPROM.read(22);
1092 uint8_t len3 = EEPROM.read(23);
1093 uint16_t wp = 65535;
1094
1095 if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3 > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3 > 250 - len2) || (len1 + len2 > 250))
1096 b3 = len1 = len2 = len3 = 0;
1097
1098 if (slot == 1)
1099 {
1100 if (((ch_len < b3) || (len3 == 0)) && (ch_len + len2 + len3 <= 250))
1101 {
1102 EEPROM.write(21, ch_len);
1103 wp = 24;
1104 }
1105 }
1106 else if (slot == 2)
1107 {
1108 if ((250 - b3 - len3 > ch_len) && (ch_len + len1 + len3 <= 250))
1109 {
1110 EEPROM.write(22, ch_len);
1111 wp = 1000 - ch_len * 4;
1112 }
1113 }
1114 else if (slot == 3)
1115 {
1116 if (ch_len + len1 + len2 <= 250)
1117 {
1118 EEPROM.write(23, ch_len);
1119 EEPROM.write(0, (len1 - len2) / 2 + 125);
1120 wp = 4 * ((len1 - len2) / 2 + 125);
1121 }
1122 }
1123
1124 if (wp == 65535)
1125 serial_println_flash(PSTR("not enough space"));
1126 else
1127 {
1128 for (int i = 0; i < ch_len; i++)
1129 {
1130 EEPROM.write(wp + 4 * i, ch_time[i] & 255);
1131 EEPROM.write(wp + 1 + 4 * i, ch_time[i] >> 8);
1132 EEPROM.write(wp + 2 + 4 * i, ch_servo[i]);
1133 EEPROM.write(wp + 3 + 4 * i, ch_val[i]);
1134 }
1135 serial_println_flash(PSTR("ok"));
1136 }
1137 }
1138
1139 void precitaj_choreografiu_z_EEPROM(uint8_t slot)
1140 {
1141 uint8_t b3 = EEPROM.read(0);
1142 uint8_t len1 = EEPROM.read(21);
1143 uint8_t len2 = EEPROM.read(22);
1144 uint8_t len3 = EEPROM.read(23);
1145 uint16_t rp = 65535;
1146 if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3 > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3 > 250 - len2) || (len1 + len2 > 250))
1147 b3 = len1 = len2 = len3 = 0;
1148
1149 if (slot == 1)
1150 {
1151 if (len1 > 0)
1152 {
1153 rp = 24;
1154 ch_len = len1;
1155 }
1156 }
1157 else if (slot == 2)
1158 {
1159 if (len2 > 0)
1160 {
1161 ch_len = len2;
1162 rp = 1000 - ch_len * 4;
1163 }
1164 }
1165 else if (slot == 3)
1166 {
1167 if (len3 > 0)
1168 {
1169 rp = b3 * 4;
1170 ch_len = len3;
1171 }
1172 }
1173
1174 if (rp == 65535)
1175 serial_println_flash(PSTR("couldn't"));
1176 else
1177 {
1178 for (int i = 0; i < ch_len; i++)
1179 {
1180 ch_time[i] = ((uint16_t)EEPROM.read(rp + 4 * i)) |
1181 (((uint16_t)EEPROM.read(rp + 1 + 4 * i)) << 8);
1182 ch_servo[i] = EEPROM.read(rp + 2 + 4 * i);
1183 ch_val[i] = EEPROM.read(rp + 3 + 4 * i);
1184 }
1185 serial_println_flash(PSTR("ok"));
1186 pipni();
1187 }
1188 }
1189
1190 void naformatuj_EEPROM_choreografie()
1191 {
1192 EEPROM.write(0, 0);
1193 EEPROM.write(21, 0);
1194 EEPROM.write(22, 0);
1195 EEPROM.write(23, 0);
1196 serial_println_flash(PSTR("ok"));
1197 pipni();
1198 }
1199
1200 void precitaj_kalibraciu_z_EEPROM()
1201 {
1202 uint8_t value = EEPROM.read(1);
1203 if ((value != '~') && (value != '1') && (value != '2') && (value != '3')) return;
1204 if (value != '~') auto_start = value - '0';
1205 else auto_start = 0;
1206 for (int i = 2; i < 8; i++)
1207 {
1208 int16_t k = EEPROM.read(i);
1209 if (k > 127) servo_invertovane[i - 2] = 1;
1210 else servo_invertovane[i - 2] = 0;
1211 k &= 127;
1212 prednastavena_kalibracia[i - 2] = k + (90-63);
1213 }
1214 for (int i = 0; i < 6; i++)
1215 dolny_limit[i] = EEPROM.read(i + 9);
1216 for (int i = 0; i < 6; i++)
1217 horny_limit[i] = EEPROM.read(i + 15);
1218 }
1219
1220 void zapis_kalibraciu_do_EEPROM()
1221 {
1222 serial_print_flash(PSTR("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: "));
1223 char odpoved = read_char();
1224 serial_println_char(odpoved);
1225 if (odpoved == 'Y')
1226 {
1227 char kalib_state = EEPROM.read(1);
1228 if ((kalib_state == '~') || ((kalib_state >= '1') && (kalib_state <= '3')))
1229 EEPROM.write(1, kalib_state);
1230 else EEPROM.write(1, '~');
1231 for (int i = 2; i < 8; i++)
1232 {
1233 int16_t k = kalib[i - 2] - (90-63);
1234 if (k < 0) k = 0;
1235 else if (k > 127) k = 127;
1236 if (servo_invertovane[i - 2]) k += 128;
1237 EEPROM.write(i, (uint8_t)k);
1238 }
1239 for (int i = 0; i < 6; i++)
1240 EEPROM.write(9 + i, dolny_limit[i]);
1241 for (int i = 0; i < 6; i++)
1242 EEPROM.write(15 + i, horny_limit[i]);
1243 serial_println_flash(PSTR("ok"));
1244 }
1245 }
1246
1247 // nasleduje softverova implementacia serioveho portu
1248 #define SERIAL_STATE_IDLE 0
1249 #define SERIAL_STATE_RECEIVING 1
1250 #define SERIAL_BUFFER_LENGTH 20
1251
1252 static volatile uint8_t serial_state;
1253 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
1254 static volatile uint8_t serial_buf_wp, serial_buf_rp;
1255
1256 static volatile uint8_t receiving_byte;
1257
1258 static volatile uint32_t time_startbit_noticed;
1259 static volatile uint8_t next_bit_order;
1260 static volatile uint8_t waiting_stop_bit;
1261 static uint16_t one_byte_duration;
1262 static uint16_t one_bit_duration;
1263 static uint16_t one_bit_write_duration;
1264 static uint16_t half_of_one_bit_duration;
1265
1266 void init_serial(uint32_t baud_rate)
1267 {
1268 pinMode(2, INPUT);
1269 pinMode(4, OUTPUT);
1270
1271 serial_state = SERIAL_STATE_IDLE;
1272
1273 one_byte_duration = 9500000 / baud_rate;
1274 one_bit_duration = 1000000 / baud_rate;
1275 one_bit_write_duration = one_bit_duration - 1;
1276 half_of_one_bit_duration = 500000 / baud_rate;
1277
1278 PCMSK2 |= 4; //PCINT18;
1279 PCIFR &= ~4; //PCIF2;
1280 PCICR |= 4; // PCIE2;
1281 }
1282
1283 ISR(PCINT2_vect)
1284 {
1285 uint32_t tm = micros();
1286 if (serial_state == SERIAL_STATE_IDLE)
1287 {
1288 time_startbit_noticed = tm;
1289 serial_state = SERIAL_STATE_RECEIVING;
1290 receiving_byte = 0xFF;
1291 next_bit_order = 0;
1292 }
1293 else if (tm - time_startbit_noticed > one_byte_duration)
1294 {
1295 serial_buffer[serial_buf_wp] = receiving_byte;
1296 serial_buf_wp++;
1297 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1298 time_startbit_noticed = tm;
1299 receiving_byte = 0xFF;
1300 next_bit_order = 0;
1301 }
1302 else if (PIND & 4)
1303 {
1304 int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1305 while (next_bit_order < new_next_bit_order)
1306 {
1307 receiving_byte &= ~(1 << next_bit_order);
1308 next_bit_order++;
1309 }
1310 if (next_bit_order == 8)
1311 {
1312 serial_buffer[serial_buf_wp] = receiving_byte;
1313 serial_buf_wp++;
1314 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1315 serial_state = SERIAL_STATE_IDLE;
1316 }
1317 } else
1318 next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1319 }
1320
1321 uint8_t serial_available()
1322 {
1323 cli();
1324 if (serial_buf_rp != serial_buf_wp)
1325 {
1326 sei();
1327 return 1;
1328 }
1329 if (serial_state == SERIAL_STATE_RECEIVING)
1330 {
1331 uint32_t tm = micros();
1332 if (tm - time_startbit_noticed > one_byte_duration)
1333 {
1334 serial_state = SERIAL_STATE_IDLE;
1335 serial_buffer[serial_buf_wp] = receiving_byte;
1336 serial_buf_wp++;
1337 if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1338 sei();
1339 return 1;
1340 }
1341 }
1342 sei();
1343 return 0;
1344 }
1345
1346 int16_t serial_read()
1347 {
1348 cli();
1349 if (serial_buf_rp != serial_buf_wp)
1350 {
1351 uint8_t ch = serial_buffer[serial_buf_rp];
1352 serial_buf_rp++;
1353 if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
1354 sei();
1355 return ch;
1356 }
1357
1358 if (serial_state == SERIAL_STATE_RECEIVING)
1359 {
1360 uint32_t tm = micros();
1361 if (tm - time_startbit_noticed > one_byte_duration)
1362 {
1363 uint8_t ch = receiving_byte;
1364 serial_state = SERIAL_STATE_IDLE;
1365 sei();
1366 return ch;
1367 }
1368 }
1369 sei();
1370 return -1;
1371 }
1372
1373 void serial_write(uint8_t ch)
1374 {
1375 #ifdef ECHO_BT_TO_USB
1376 Serial.print((char)ch);
1377 #endif
1378 PORTD &= ~16;
1379 delayMicroseconds(one_bit_write_duration);
1380 for (uint8_t i = 0; i < 8; i++)
1381 {
1382 if (ch & 1) PORTD |= 16;
1383 else PORTD &= ~16;
1384 ch >>= 1;
1385 delayMicroseconds(one_bit_write_duration);
1386 }
1387 PORTD |= 16;
1388 delayMicroseconds(one_bit_write_duration);
1389 delayMicroseconds(one_bit_write_duration);
1390 delayMicroseconds(one_bit_write_duration);
1391 delayMicroseconds(one_bit_write_duration);
1392 delayMicroseconds(one_bit_write_duration);
1393 }
1394
1395 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
1396 {
1397 uint16_t len;
1398 int16_t ch;
1399 do {
1400 ch = serial_read();
1401 if (ch == 13) continue;
1402 } while (ch == -1);
1403
1404 do {
1405 if ((ch != 13) && (ch != 10) && (ch != -1))
1406 {
1407 *(ln++) = ch;
1408 max_length--;
1409 len++;
1410 }
1411 ch = serial_read();
1412 } while ((ch != 13) && max_length);
1413 *ln = 0;
1414 return len;
1415 }
1416
1417 void serial_print_num(int32_t number)
1418 {
1419 if (number < 0)
1420 {
1421 serial_write('-');
1422 number = -number;
1423 }
1424 int32_t rad = 1;
1425 while (number / rad) rad *= 10;
1426 if (number > 0) rad /= 10;
1427 while (rad)
1428 {
1429 serial_write((char)('0' + (number / rad)));
1430 number -= (number / rad) * rad;
1431 rad /= 10;
1432 }
1433 }
1434
1435 void serial_print_char(char ch)
1436 {
1437 serial_write(ch);
1438 }
1439
1440 void serial_print(const char *str)
1441 {
1442 while (*str) serial_write(*(str++));
1443 }
1444
1445 void serial_println(const char *str)
1446 {
1447 serial_print(str);
1448 serial_write(13);
1449 serial_write(10);
1450 }
1451
1452 void serial_print_flash(const char *str)
1453 {
1454 int ln = strlen_P(str);
1455 for (int i = 0; i < ln; i++)
1456 serial_write(pgm_read_byte(str + i));
1457 }
1458
1459 void serial_println_flash(const char *str)
1460 {
1461 serial_print_flash(str);
1462 serial_write(13);
1463 serial_write(10);
1464 }
1465
1466 void serial_println_num(int32_t number)
1467 {
1468 serial_print_num(number);
1469 serial_println();
1470 }
1471
1472 void serial_println_char(char ch)
1473 {
1474 serial_write(ch);
1475 serial_println();
1476 }
1477
1478 void serial_println()
1479 {
1480 serial_write(13);
1481 serial_write(10);
1482 }
1483
1484 // nasleduje citanie z utltazvukoveho senzora
1485
1486 static volatile uint32_t pulse_start;
1487 static volatile uint8_t new_distance;
1488
1489 void init_ultrasonic()
1490 {
1491 pinMode(US_ECHO, INPUT);
1492 pinMode(US_TRIG, OUTPUT);
1493
1494 PCMSK0 |= 1; //PCINT0;
1495 PCIFR &= ~1; //PCIF0;
1496 PCICR |= 1; // PCIE0;
1497 }
1498
1499 ISR(PCINT0_vect)
1500 {
1501 if (PINB & 1) pulse_start = micros();
1502 else
1503 {
1504 distance = (int16_t)((micros() - pulse_start) / 58);
1505 new_distance = 1;
1506 }
1507 }
1508
1509 void start_distance_measurement()
1510 {
1511 distance = 10000;
1512 new_distance = 0;
1513 digitalWrite(US_TRIG, HIGH);
1514 delayMicroseconds(10);
1515 digitalWrite(US_TRIG, LOW);
1516 }
1517
1518 void wait_for_distance_measurement_to_complete()
1519 {
1520 uint8_t counter = 0;
1521 while ((counter < 20) && !new_distance)
1522 {
1523 delay(1);
1524 counter++;
1525 }
1526 if (counter == 20)
1527 {
1528 pinMode(US_ECHO, OUTPUT);
1529 digitalWrite(US_ECHO, HIGH);
1530 delayMicroseconds(10);
1531 digitalWrite(US_ECHO, LOW);
1532 pinMode(US_ECHO, INPUT);
1533 delayMicroseconds(5);
1534 distance = 10000;
1535 }
1536 }
1537
1538 int16_t measure_distance()
1539 {
1540 start_distance_measurement();
1541 wait_for_distance_measurement_to_complete();
1542 return distance;
1543 }
1544
1545 //-------------------------------- nasleduje prehravanie melodie a hranie cez timer2 v pozadi
1546 #define SIRENE_PORT PORTB
1547 #define SIRENE_DDR DDRB
1548 #define SIRENE_PIN 4
1549
1550 #define FIS3 2960
1551 #define G3 3136
1552
1553 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 };
1554
1555 //popcorn
1556 uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217};
1557 const uint8_t melodia1[] PROGMEM = { 252, 50, 149, 49,
1558 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,
1559 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,
1560 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,
1561 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,
1562 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,
1563 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,
1564 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,
1565 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 ,
1566 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,
1567 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,
1568 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 ,
1569 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
1570 };
1571
1572 //kohutik jarabi
1573 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 };
1574
1575 //kankan
1576 const uint8_t melodia3[] PROGMEM = { 252, 100,
1577 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,
1578 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,
1579 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,
1580 135, 131, 128, 126, 75, 76, 78, 80, 81, 76, 80, 76, 81, 76, 80, 76, 81, 76, 80, 76, 81, 76, 80, 76,
1581 251, 2, 11, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1582 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1583 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1584 251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1585 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,
1586 174, 88, 91, 90, 88, 143, 143, 93, 95, 90, 91, 138, 138, 88, 91, 90, 88, 86, 93, 89, 90, 136,
1587 64, 71, 71, 73, 71, 69, 69, 73, 74, 78, 81, 78, 78, 76, 251, 1, 184, 1, 32, 126,
1588 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126,
1589 78, 76, 78, 76, 78, 76, 78, 76,
1590 131, 119, 119, 119, 169
1591 };
1592
1593 //labutie jazero
1594 const uint8_t melodia4[] PROGMEM = {
1595 252, 220, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69,
1596 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,
1597 99, 83, 81, 80,
1598 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,
1599 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,
1600 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,
1601 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,
1602 99, 83, 81, 80,
1603 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
1604 };
1605
1606 volatile int16_t music_speed = 800 / 16;
1607
1608 //volatile PGM_P current_note ;
1609 volatile const uint8_t *current_note ;
1610 volatile uint16_t notes_remaining;
1611
1612 void zahraj_melodiu(uint8_t cislo)
1613 {
1614 if (cislo == 0) {
1615 zastav_melodiu();
1616 return;
1617 }
1618
1619 if (cislo == 1) current_note = melodia1;
1620 else if (cislo == 2) current_note = melodia2;
1621 else if (cislo == 3) current_note = melodia3;
1622 else if (cislo == 4) current_note = melodia4;
1623 notes_remaining = dlzka_melodia[cislo];
1624
1625 next_note();
1626 }
1627
1628 void next_note()
1629 {
1630 uint16_t freq, dur;
1631 if (!notes_remaining) return;
1632 otto_translate_tone_flash(&freq, &dur);
1633 tone2(freq, dur);
1634 }
1635
1636 void otto_translate_tone_flash(uint16_t *freq, uint16_t *del)
1637 {
1638 do {
1639 uint8_t n = pgm_read_byte(current_note);
1640 if (n == 251)
1641 {
1642 current_note++;
1643 uint8_t f1 = pgm_read_byte(current_note);
1644 current_note++;
1645 uint8_t f2 = pgm_read_byte(current_note);
1646 current_note++;
1647 uint8_t d1 = pgm_read_byte(current_note);
1648 current_note++;
1649 uint8_t d2 = pgm_read_byte(current_note);
1650 *freq = (f1 << 8) + f2;
1651 *del = (music_speed * 16 * (long)d1) / d2;
1652 notes_remaining -= 4;
1653 }
1654 else if (n == 252)
1655 {
1656 current_note++;
1657 music_speed = pgm_read_byte(current_note);
1658 current_note++;
1659 notes_remaining -= 2;
1660 continue;
1661 }
1662 else if (n == 254)
1663 {
1664 *freq = FIS3;
1665 *del = music_speed;
1666 }
1667 else if (n == 255)
1668 {
1669 *freq = G3;
1670 *del = music_speed;
1671 }
1672 else
1673 {
1674 uint8_t len = n / 50;
1675 *del = music_speed;
1676 while (len--) *del *= 2;
1677 n = n % 50;
1678 if (n != 49)
1679 {
1680 uint8_t octave = (n + 5) / 12;
1681 n = (n + 5) % 12;
1682 float ffreq = octave_4[n];
1683 octave = 4 - octave;
1684 while (octave > 0)
1685 {
1686 ffreq /= 2.0;
1687 octave--;
1688 }
1689 *freq = (uint16_t)ffreq;
1690 }
1691 else *freq = 0;
1692 }
1693 notes_remaining--;
1694 current_note++;
1695 break;
1696 } while (1);
1697 }
1698
1699 static volatile uint8_t tone2_state;
1700 static volatile uint8_t tone2_pause;
1701 static volatile uint32_t tone2_len;
1702
1703 void init_tone2()
1704 {
1705 notes_remaining = 0;
1706 tone2_pause = 0;
1707 TCCR2A = 2;
1708 TCCR2B = 0;
1709 TIMSK2 = 2;
1710 SIRENE_DDR |= (1 << SIRENE_PIN);
1711 }
1712
1713 ISR(TIMER2_COMPA_vect)
1714 {
1715 if (!tone2_pause)
1716 {
1717 if (tone2_state)
1718 {
1719 SIRENE_PORT |= (1 << SIRENE_PIN);
1720 tone2_state = 0;
1721 }
1722 else
1723 {
1724 SIRENE_PORT &= ~(1 << SIRENE_PIN);
1725 tone2_state = 1;
1726 }
1727 }
1728 if ((--tone2_len) == 0)
1729 {
1730 TCCR2B = 0;
1731 tone2_pause = 0;
1732 next_note();
1733 }
1734 }
1735
1736 void tone2(uint16_t freq, uint16_t duration)
1737 {
1738 uint32_t period = ((uint32_t)1000000) / (uint32_t)freq;
1739
1740 if (freq >= 977) // prescaler 32
1741 {
1742 tone2_state = 0;
1743 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1744 if (tone2_len == 0) tone2_len++;
1745 TCNT2 = 0;
1746 OCR2A = (uint8_t) (250000 / (uint32_t)freq);
1747 TCCR2B = 3;
1748 }
1749 else if (freq >= 488) // prescaler 64
1750 {
1751 tone2_state = 0;
1752 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1753 if (tone2_len == 0) tone2_len++;
1754 TCNT2 = 0;
1755 OCR2A = (uint8_t) (125000 / (uint32_t)freq);
1756 TCCR2B = 4;
1757 }
1758 else if (freq >= 244) // prescaler 128
1759 {
1760 tone2_state = 0;
1761 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1762 if (tone2_len == 0) tone2_len++;
1763 TCNT2 = 0;
1764 OCR2A = (uint8_t) (62500 / (uint32_t)freq);
1765 TCCR2B = 5;
1766 }
1767 else if (freq >= 122) //prescaler 256
1768 {
1769 tone2_state = 0;
1770 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1771 if (tone2_len == 0) tone2_len++;
1772 TCNT2 = 0;
1773 OCR2A = (uint8_t) (31250 / (uint32_t)freq);
1774 TCCR2B = 6;
1775 }
1776 else if (freq >= 30) //prescaler 1024
1777 {
1778 tone2_state = 0;
1779 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1780 if (tone2_len == 0) tone2_len++;
1781 TCNT2 = 0;
1782 OCR2A = (uint8_t) (7813 / (uint32_t)freq);
1783 TCCR2B = 7;
1784 }
1785 else if (freq == 0)
1786 {
1787 tone2_pause = 1;
1788 tone2_state = 0;
1789 period = 1000000 / 500;
1790 tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1791 TCNT2 = 0;
1792 OCR2A = (uint8_t) (125000 / (uint32_t)500);
1793 TCCR2B = 4;
1794 }
1795 else
1796 {
1797 TCCR2B = 0;
1798 }
1799 }
1800
1801 void zastav_melodiu()
1802 {
1803 notes_remaining = 0;
1804 }