Otto - riadiaci program v.4

From DT^2
Revision as of 09:53, 10 August 2018 by Admin (talk | contribs)
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

Čí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)

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