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