Difference between revisions of "Otto - riadiaci program v.4"

From DT^2
Jump to: navigation, search
m
m
Line 72: Line 72:
 
# podrž klávesy 'q', 'a' - mal by hýbať rukou
 
# podrž klávesy 'q', 'a' - mal by hýbať rukou
 
# stlač kláves 'X' na preformátovanie tabuľky v EEPROM (na všetky potvrdenia y/n záleží na tom, či je malé velké písmeno - čiže potvrdzuješ iba veľkým 'Y' //sorry, malo tam byť skôr 'A' :)
 
# stlač kláves 'X' na preformátovanie tabuľky v EEPROM (na všetky potvrdenia y/n záleží na tom, či je malé velké písmeno - čiže potvrdzuješ iba veľkým 'Y' //sorry, malo tam byť skôr 'A' :)
# stlač kláves 'H' a postupuj podľa pokynov pri kalibrácii - vždy nastavuješ neutrálnu polohu - vzpriamené ruky, zarovnané nohy, šľapy sú vodorovne na zemi
+
# stlač kláves 'H' a postupuj podľa pokynov pri kalibrácii - vždy nastavuješ neutrálnu polohu - upažené ruky, zarovnané nohy, šľapy sú vodorovne na zemi
 
# stlač kláves 'W' na uloženie kalibrácie do EEPROM
 
# stlač kláves 'W' na uloženie kalibrácie do EEPROM
 
# odpoj USB
 
# odpoj USB

Revision as of 12:32, 21 November 2018

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:

pin signál
10 ľavá ruka
11 pravá ruka
9 ľavá noha
6 pravá noha
5 ľavá päta
3 pravá päta
2 TXD BlueTooth
4 RXD BlueTooth
7 TRIG Ultrazvuk
8 ECHO Ultrazvuk
12 Siréna +
G Siréna -, GND Ultrazvuk, GND BlueTooth, kondenzátor -, najtmavší káblik každého serva, oba kábliky z vypínača
V VCC Ultrazvuk, VCC BlueTooth, kondenzátor +, červený káblik každého serva, oba kabliky z diody

1 HC-SR04 Front Back.jpeg

Číslovanie serva v choreografii:

servo číslo serva v choreografii
ľavá ruka 5
pravá ruka 6
ľavá noha 4
pravá noha 3
ľavá päta 2
pravá päta 1

Spojazdenie robota:

  1. ešte raz dôkladne skontroluj, či máš všetky kábliky pripojené správne - servokábliky musia byť otočené tak, že hnedý ide na zem (G)!
  2. robota budeš mať zatiaľ vypnutého (vypínač je OFF), pri konfigurácii sa bude napájať z USB
  3. downloadni si PUTTY, ak ho ešte nemáš
  4. downloadni si Arduino IDE, ak ho ešte nemáš
  5. v Arduino IDE otvor program, ktorý je zalinkovaný nižšie (ottoX.ino)
  6. pripoj Arduino cez USB káblik k PC a v Device Manageri pohľadaj, ktorý sériový port sa vytvoril (CH340) - napr. COM3. (tieto tri kroky odporúčam robiť s Arduinom, kým nie je pripojené do rozširujúcej dosky)
  7. nastav port (Tools-Port) podľa predchádzajúceho bodu a Arduino Nano (Tools-Board)
  8. uploadni program do robota - ak to nejde, skús vybrať Arduino z dosky a uploadnuť program do voľného nepripojeného Arduina - ak to ide, tak si na doske niečo zle pripojil, ak to nejde, tak je buď Arduino už pokazené, alebo si vybral nesprávny port, alebo dosku, alebo USB káblik nemáš dobre pripojený
  9. po úspešnom uploade sa robot ozve (ak sa neozve, sirénka nie je dobre pripojená)
  10. otvor putty, vyber "serial", zadaj názov portu (napr. COM3) a nastav 9600 BPS, pripoj sa - robot by sa mal znova ozvať, lebo Arduino sa pri otváraní sériového portu vždy resetne
  11. stlač kláves '-' a robot by mal písknuť
  12. podrž klávesy 'q', 'a' - mal by hýbať rukou
  13. stlač kláves 'X' na preformátovanie tabuľky v EEPROM (na všetky potvrdenia y/n záleží na tom, či je malé velké písmeno - čiže potvrdzuješ iba veľkým 'Y' //sorry, malo tam byť skôr 'A' :)
  14. stlač kláves 'H' a postupuj podľa pokynov pri kalibrácii - vždy nastavuješ neutrálnu polohu - upažené ruky, zarovnané nohy, šľapy sú vodorovne na zemi
  15. stlač kláves 'W' na uloženie kalibrácie do EEPROM
  16. odpoj USB
  17. info: keď budeš mať otvorené putty okno cez USB, tak cez Arduino nejde programovať - najskôr treba zavrieť okno Putty (pre BT to neplatí, BT okno môže ostať otvorené počas programovania, programovať sa dá len cez USB)
  18. zapni robota, mal by sa ozvať a dať do nakalibrovanej polohy
  19. zapni si BT na notebooku a vyhľadaj zariadenia BlueTooth - pripoj HC05, párovací kód je 1234
  20. v nastaveniach BT si zisti na aký port (výstupný/outgoing) ti HC05 pripojilo
  21. otvor si putty a nastav serial, tento port a 9600
  22. gratulujem, veselú zábavu... (pozri si choreografie, ich formát, vytvor si nové pesničky, pripoj si ho cez android - vysielaj znaky 3,4,5,6 - bude chodiť rôznymi smermi)
  23. robot sám od seba rád škrieka, keď sa nudí, tak odporúčam vypínať režim nudy (alebo si to zmeň v programe), keby niečo nešlo, napíš Paľovi - ppetrovic@acm.org
kláves 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í choreografie)
medzera reset všetkých motorov do strednej polohy
3,...,6 zmiešaný pohyb (treba nastaviť vo funkciách kombinacia1() - kombinacia4())
v,b,n,j,h,g,f,s,l,o,i,u,y,r,e,w zvukové efekty 1-16
TAB,*,/,+ zahrá melódiu 1-4
W zapísať choreografiu do EEPROM (opatrne)
C načítať choreografiu z EEPROM
X formátovať EEPROM na zápis choreografií (opatrne)
V vypni/zapni režim nudy
A automatický štart choreografie po zapnutí

Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií

Download: otto4.ino

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