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