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