Otto - riadiaci program v.4

From DT^2
Revision as of 13:31, 10 August 2018 by 158.195.160.45 (talk)
Jump to: navigation, search

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

1 HC-SR04 Front Back.jpeg

Čí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
  • pomocou + a - nájdeme strednú polohu serva
  • enter potvrdí hodnotu a pokračuje sa ďalším servom
  • kalibrácia sa po vypnutí nezapamätá, pokiaľ ju neuložíte do EEPROM (pozri nižšie)
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)
V vypni/zapni režim nudy

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