Otto - riadiaci program v.4

From DT^2
Revision as of 09:33, 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: otto3.ino

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