Otto - riadiaci program v.4

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