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

From DT^2
Jump to: navigation, search
m
m
Line 194: Line 194:
 
uint8_t auto_start;
 
uint8_t auto_start;
  
static volatile int16_t distance;
+
volatile int16_t distance;
static uint32_t cas_ked_naposledy_stlacil;
+
uint32_t cas_ked_naposledy_stlacil;
static uint8_t vypni_nudu;
+
uint8_t vypni_nudu;
  
 
void setup() {
 
void setup() {
Line 673: Line 673:
 
   if (cmd == 9) zahraj_melodiu(3);
 
   if (cmd == 9) zahraj_melodiu(3);
 
   if (cmd == 10) zahraj_melodiu(4);
 
   if (cmd == 10) zahraj_melodiu(4);
 +
  if (cmd == 11) zahraj_melodiu(5);
 
}
 
}
  
Line 1,680: Line 1,681:
  
 
//popcorn
 
//popcorn
uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217, 21};
+
uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217, 36};
 
const uint8_t melodia1[] PROGMEM = { 252, 50, 149,  49,
 
const uint8_t melodia1[] PROGMEM = { 252, 50, 149,  49,
 
                                     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,
 
                                     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,
Line 1,730: Line 1,731:
 
};
 
};
  
//let it be
+
//let it be https://www.musicnotes.com/sheetmusic/mtd.asp?ppn=MN0101556
const uint8_t melodia5[] PROGMEM = { 252,200,26,26,76,26,28,78,73,76,76,31,83,35,85,35,85,83,83,81,131 };
+
const uint8_t melodia5[] PROGMEM = { 252, 200, 26, 26, 76, 26, 253, 78, 73, 76, 76, 31, 253, 83, 35, 85, 253, 85,
 +
                                    83, 83, 81, 131, 35, 253, 85, 86, 35, 35, 85, 83, 99, 35, 33, 83, 81, 181
 +
};
  
 
volatile int16_t music_speed = 800 / 16;
 
volatile int16_t music_speed = 800 / 16;
 
//volatile PGM_P current_note ;
 
 
volatile const uint8_t *current_note ;
 
volatile const uint8_t *current_note ;
 
volatile uint16_t notes_remaining;
 
volatile uint16_t notes_remaining;
 +
uint8_t dotted_note = 0;
  
 
void zahraj_melodiu(uint8_t cislo)
 
void zahraj_melodiu(uint8_t cislo)
Line 1,758: Line 1,760:
 
void next_note()
 
void next_note()
 
{
 
{
   uint16_t freq, dur;
+
   uint16_t freq = 0, dur = 0;
 
   if (!notes_remaining) return;
 
   if (!notes_remaining) return;
 
   otto_translate_tone_flash(&freq, &dur);
 
   otto_translate_tone_flash(&freq, &dur);
Line 1,768: Line 1,770:
 
   do {
 
   do {
 
     uint8_t n = pgm_read_byte(current_note);
 
     uint8_t n = pgm_read_byte(current_note);
     if (n == 251)
+
     if (n > 249)
 
     {
 
     {
       current_note++;
+
       if (n == 251)
      uint8_t f1 = pgm_read_byte(current_note);
+
      {
      current_note++;
+
        current_note++;
      uint8_t f2 = pgm_read_byte(current_note);
+
        uint8_t f1 = pgm_read_byte(current_note);
      current_note++;
+
        current_note++;
      uint8_t d1 = pgm_read_byte(current_note);
+
        uint8_t f2 = pgm_read_byte(current_note);
      current_note++;
+
        current_note++;
      uint8_t d2 = pgm_read_byte(current_note);
+
        uint8_t d1 = pgm_read_byte(current_note);
      *freq = (f1 << 8) + f2;
+
        current_note++;
      *del = (music_speed * 16 * (long)d1) / d2;
+
        uint8_t d2 = pgm_read_byte(current_note);
      notes_remaining -= 4;
+
        *freq = (f1 << 8) + f2;
    }
+
        *del = (music_speed * 16 * (long)d1) / d2;
    else if (n == 252)
+
        notes_remaining -= 4;
    {
+
      }
      current_note++;
+
      else if (n == 252)
      music_speed = pgm_read_byte(current_note);
+
      {
      current_note++;
+
        current_note++;
      notes_remaining -= 2;
+
        music_speed = pgm_read_byte(current_note);
       continue;
+
        current_note++;
    }
+
        notes_remaining -= 2;
    else if (n == 254)
+
        continue;
    {
+
      }
      *freq = FIS3;
+
      else if (n == 253)
      *del = music_speed;
+
       {
    }
+
        dotted_note = 1;
    else if (n == 255)
+
        current_note++;
    {
+
        notes_remaining--;       
      *freq = G3;
+
        continue;
      *del = music_speed;
+
      }
 +
      else if (n == 254)
 +
      {
 +
        *freq = FIS3;
 +
        *del = music_speed;
 +
      }
 +
      else if (n == 255)
 +
      {
 +
        *freq = G3;
 +
        *del = music_speed;
 +
      }
 
     }
 
     }
 
     else
 
     else
Line 1,825: Line 1,837:
 
     break;
 
     break;
 
   } while (1);
 
   } while (1);
 +
  if (dotted_note)
 +
  {
 +
    *del += (*del) / 2;
 +
    dotted_note = 0;
 +
  }
 
}
 
}
  
Line 1,835: Line 1,852:
 
   notes_remaining = 0;
 
   notes_remaining = 0;
 
   tone2_pause = 0;
 
   tone2_pause = 0;
 +
  dotted_note = 0;
 
   TCCR2A = 2;
 
   TCCR2A = 2;
 
   TCCR2B = 0;
 
   TCCR2B = 0;

Revision as of 17:04, 1 September 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
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í 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"));
 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 void vpred()
 582 {
 583   while ((measure_distance() > 30) && !serial_available() && !Serial.available())
 584     zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
 585   pipni();
 586   reset();
 587 }
 588 
 589 void pipni()
 590 {
 591   tone2( 1568, 50);
 592   delay(100);
 593   tone2( 1357, 50);
 594 }
 595 
 596 void ruky()
 597 {
 598   uint8_t odloz_spomalenie = spomalenie;
 599   spomalenie = 0;
 600 #ifdef ROBOT_3D_PRINTED
 601   nastav_koncatinu(5, 180);
 602   nastav_koncatinu(6, 180);
 603 #else
 604   nastav_koncatinu(5, 0);
 605   nastav_koncatinu(6, 0);
 606 #endif
 607   delay(1000);
 608   nastav_koncatinu(5, 90);
 609   nastav_koncatinu(6, 90);
 610   spomalenie = odloz_spomalenie;
 611   delay(1000);
 612   pipni();
 613 }
 614 
 615 void ahoj()
 616 {
 617   tone2( 1568, 50);
 618   delay(70);
 619   tone2( 1175, 30);
 620   delay(50);
 621   tone2( 880, 30);
 622   delay(50);
 623   tone2( 1047, 50);
 624   delay(70);
 625   tone2( 1245, 30);
 626   delay(150);
 627   tone2( 1568, 50);
 628   delay(100);
 629   if (random(10) > 4) tone2( 1357, 50);
 630   else tone2( 1047, 50);
 631 }
 632 
 633 void nastav_koncatinu(int8_t srv, uint8_t poloha)
 634 {
 635   int8_t delta;
 636   srv--;
 637 
 638   if (servo_invertovane[srv]) poloha = 180 - poloha;
 639   
 640   if ((int16_t)poloha + (int16_t)kalib[srv] - 90 < 0) poloha = 0;
 641   else poloha += kalib[srv] - 90;
 642   
 643   if (poloha > 180) poloha = 180;
 644 
 645   if (spomalenie == 0) {
 646     stav[srv] = poloha;
 647     s[srv].write(stav[srv]);
 648   }
 649   else {  
 650     if (stav[srv] < poloha) delta = 1;
 651     else delta = -1;
 652     while (stav[srv] != poloha)
 653     {
 654       stav[srv] += delta;
 655       s[srv].write(stav[srv]);
 656       delay(spomalenie);
 657     }
 658   }
 659 }
 660 
 661 void zobraz_stav()
 662 {
 663   for (int i = 0; i < 6; i++)
 664   {
 665     serial_print_flash(PSTR("S")); serial_print_num(i + 1); serial_print_flash(PSTR(": ")); serial_println_num(stav[i] - kalib[i] + 90);
 666   }
 667   serial_println_flash(PSTR("---"));
 668   pipni();
 669 }
 670 
 671 void pohyb(int8_t servo)
 672 {
 673   int8_t srv = (servo > 0) ? servo : -servo;
 674   srv--;
 675   if (servo_invertovane[srv]) servo = -servo;
 676   if (servo > 0)
 677   {
 678     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
 679     else stav[srv] = horny_limit[srv];
 680     s[srv].write(stav[srv]);
 681   }
 682   else if (servo < 0)
 683   {
 684     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok;
 685     else stav[srv] = dolny_limit[srv];
 686     s[srv].write(stav[srv]);
 687   }
 688 }
 689 
 690 uint8_t pohyb_znakom(char z)
 691 {
 692   for (int i = 0; i < 12; i++)
 693   {
 694     if (z == znaky_zmien[i])
 695     {
 696       int8_t servo = zmeny[i];
 697       pohyb(servo);
 698       return 1;
 699     }
 700   }
 701   return 0;
 702 }
 703 
 704 void kombinacia1()
 705 {
 706   pohyb(SERVO_LAVA_NOHA);
 707   pohyb(-SERVO_PRAVA_PATA);
 708 }
 709 
 710 void kombinacia2()
 711 {
 712   pohyb(SERVO_PRAVA_NOHA);
 713   pohyb(-SERVO_LAVA_PATA);
 714 }
 715 
 716 void kombinacia3()
 717 {
 718   pohyb(SERVO_LAVA_RUKA);
 719   pohyb(SERVO_PRAVA_RUKA);
 720 }
 721 
 722 void kombinacia4()
 723 {
 724   pohyb(-SERVO_LAVA_RUKA);
 725   pohyb(-SERVO_PRAVA_RUKA);
 726 }
 727 
 728 int pohyb_kombinacia(char z)
 729 {
 730   if (z == '3') kombinacia1();
 731   else if (z == '4') kombinacia2();
 732   else if (z == '5') kombinacia3();
 733   else if (z == '6') kombinacia4();
 734   else return 0;
 735   return 1;
 736 }
 737 
 738 char read_char()
 739 {
 740   while (!serial_available() && !Serial.available());
 741   if (serial_available()) return serial_read();
 742   else return Serial.read();
 743 }
 744 
 745 int nacitajCislo()
 746 {
 747   int num = 0;
 748   int z;
 749   do {
 750     z = read_char();
 751     if (z == '#') while (z != 13) z = read_char();
 752   } while ((z < '0') || (z > '9'));
 753   while ((z >= '0') && (z <= '9'))
 754   {
 755     num *= 10;
 756     num += (z - '0');
 757     do {
 758       z = read_char();
 759       if (z == -1) delayMicroseconds(10);
 760     } while (z < 0);
 761   }
 762   return num;
 763 }
 764 
 765 void nacitaj_choreografiu()
 766 {
 767   ch_len = 0;
 768   int tm;
 769   do {
 770     tm = nacitajCislo();
 771     ch_time[ch_len] = tm;
 772     ch_servo[ch_len] = nacitajCislo();
 773     ch_val[ch_len] = nacitajCislo();
 774     ch_len++;
 775     if (ch_len == CHOREO_LEN) break;
 776   } while (tm > 0);
 777   pipni();
 778 }
 779 
 780 void vypis_choreografiu()
 781 {
 782   for (int i = 0; i < ch_len; i++)
 783   {
 784     serial_print_num(ch_time[i]);
 785     serial_print(" ");
 786     serial_print_num(ch_servo[i]);
 787     serial_print(" ");
 788     serial_println_num(ch_val[i]);
 789   }
 790   serial_println_flash(PSTR("---"));
 791   pipni();
 792 }
 793 
 794 uint32_t koniec_tanca;
 795 
 796 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
 797 {
 798   koniec_tanca = millis() + 3600000;
 799   for (int i = 0; i < ch_len - 1; i++)
 800   {
 801     delay(ch_time[i]);
 802     if (millis() > koniec_tanca) break; 
 803     if (ch_servo[i] < 7) nastav_koncatinu(ch_servo[i], ch_val[i]);
 804     else i = specialny_prikaz(i, ch_servo[i], ch_val[i], ch_len);
 805     if (serial_available() || Serial.available()) break;
 806   }
 807   pipni();
 808 }
 809 
 810 int specialny_prikaz(uint16_t i, uint8_t prikaz, uint8_t argument, int len)
 811 {
 812   if (prikaz == 8) koniec_tanca = millis() + 1000 * (uint32_t)argument; 
 813   else if (prikaz == 9) 
 814   {
 815     if (serial_available() || Serial.available()) return len - 1;
 816     if (measure_distance() < 15) return len - 1;
 817     return ((int)argument) - 1;
 818   }
 819   else if (prikaz == 10) spomalenie = argument;
 820   else if (prikaz == 11) zahraj_melodiu(argument);
 821   else if (prikaz == 12) zvukovy_efekt(argument);
 822   return i;
 823 }
 824 
 825 void reset()
 826 {
 827   for (int i = 0; i < 6; i++)
 828   {
 829     stav[i] = kalib[i];
 830     s[i].write(kalib[i]);
 831     delay(300);
 832   }
 833   pipni();
 834 }
 835 
 836 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota, uint8_t min_hodnota, uint8_t max_hodnota)
 837 {
 838   serial_print_flash(PSTR(" (+/-/ENTER): "));
 839   serial_println_num(hodnota);
 840   s[servo].write(hodnota);
 841   char z;
 842   do {
 843     z = read_char();
 844     if ((z == '+') && (hodnota < max_hodnota)) hodnota++;
 845     else if ((z == '-') && (hodnota > min_hodnota)) hodnota--;
 846     if ((z == '+') || (z == '-'))
 847     {
 848       serial_print_num(hodnota); serial_print_char('\r');
 849       s[servo].write(hodnota);
 850     }
 851   } while (z != 13);
 852   return hodnota;
 853 }
 854 
 855 void kalibruj()
 856 {
 857   for (int i = 0; i < 6; i++)
 858   {
 859     serial_print_num(i);
 860     kalib[i] = nalad_hodnotu_serva(i, kalib[i], 90-63, 90+63);
 861     serial_print_num(i);
 862     serial_print(": ");
 863     serial_println_num(kalib[i]);
 864   }
 865   for (int i = 0; i < 6; i++) {
 866     serial_print_num(kalib[i]);
 867     serial_print(" ");
 868   }
 869   serial_print_flash(PSTR(" Chyt robota zozadu za telo (ENTER):"));
 870   char z;
 871   do { z = read_char(); } while (z != 13);
 872   serial_println_char(z);
 873 
 874   serial_print_flash(PSTR("*** Nasledujuce koncatiny su lave/prave z pohladu robota.\r\nAk to nesedi, treba prehodit kabliky servo!\r\n"));
 875 
 876   for (int i = 0; i < 6; i++) servo_invertovane[i] = 0;
 877  
 878   nastav_koncatinu(SERVO_PRAVA_PATA, 0);
 879   serial_print_flash(PSTR("Prava pata: je von alebo dnu? (V/D):"));
 880   do { z = read_char(); } while ((z != 'V') && (z != 'D'));
 881   serial_println_char(z);
 882   nastav_koncatinu(SERVO_PRAVA_PATA, 90);
 883   if (z == 'D') servo_invertovane[SERVO_PRAVA_PATA - 1] = 0;
 884   else servo_invertovane[SERVO_PRAVA_PATA - 1] = 1;
 885   
 886   nastav_koncatinu(SERVO_LAVA_PATA, 0);
 887   serial_print_flash(PSTR("Lava pata: je von alebo dnu? (V/D):"));
 888   do { z = read_char(); } while ((z != 'V') && (z != 'D'));
 889   serial_println_char(z);
 890   nastav_koncatinu(SERVO_LAVA_PATA, 90);
 891   if (z == 'V') servo_invertovane[SERVO_LAVA_PATA - 1] = 0;
 892   else servo_invertovane[SERVO_LAVA_PATA - 1] = 1;
 893   
 894   nastav_koncatinu(SERVO_PRAVA_NOHA, 0);
 895   serial_print_flash(PSTR("Prava noha: vpredu je pata alebo spicka? (P/S):"));
 896   do { z = read_char(); } while ((z != 'P') && (z != 'S'));
 897   serial_println_char(z);
 898   nastav_koncatinu(SERVO_PRAVA_NOHA, 90);
 899   if (z == 'P') servo_invertovane[SERVO_PRAVA_NOHA - 1] = 1;
 900   else servo_invertovane[SERVO_PRAVA_NOHA - 1] = 0;
 901 
 902   nastav_koncatinu(SERVO_LAVA_NOHA, 0);
 903   serial_print_flash(PSTR("Lava noha: vpredu je pata alebo spicka? (P/S):"));
 904   do { z = read_char(); } while ((z != 'P') && (z != 'S'));
 905   serial_println_char(z);
 906   nastav_koncatinu(SERVO_LAVA_NOHA, 90);
 907   if (z == 'S') servo_invertovane[SERVO_LAVA_NOHA - 1] = 1;
 908   else servo_invertovane[SERVO_LAVA_NOHA - 1] = 0;
 909   
 910   nastav_koncatinu(SERVO_PRAVA_RUKA, 0);
 911   serial_print_flash(PSTR("Prava ruka: je hore alebo dole? (H/D):"));
 912   do { z = read_char(); } while ((z != 'H') && (z != 'D'));
 913   serial_println_char(z);
 914   nastav_koncatinu(SERVO_PRAVA_RUKA, 90);
 915   if (z == 'H') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
 916   else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
 917 
 918   nastav_koncatinu(SERVO_LAVA_RUKA, 0);
 919   serial_print_flash(PSTR("Lava ruka: je hore alebo dole? (H/D):"));
 920   do { z = read_char(); } while ((z != 'H') && (z != 'D'));
 921   serial_println_char(z);
 922   nastav_koncatinu(SERVO_LAVA_RUKA, 90);
 923   if (z == 'D') servo_invertovane[SERVO_PRAVA_RUKA - 1] = 0;
 924   else servo_invertovane[SERVO_PRAVA_RUKA - 1] = 1;
 925 
 926   serial_println_flash(PSTR("ok"));
 927   pipni();
 928 }
 929  
 930 void nastav_limity()
 931 {
 932   for (int i = 0; i < 6; i++)
 933   {
 934     serial_print_num(i);
 935     serial_print_flash(PSTR("dolny"));
 936     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i], 0, 180);
 937     serial_print_num(i);
 938     serial_print_flash(PSTR(" dolny: "));
 939     serial_println_num(dolny_limit[i]);
 940     s[i].write(kalib[i]);
 941 
 942     serial_print_num(i);
 943     serial_print_flash(PSTR("horny"));
 944     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i], 0, 180);
 945     serial_print_num(i);
 946     serial_print_flash(PSTR(" horny: "));
 947     serial_println_num(horny_limit[i]);
 948     s[i].write(kalib[i]);
 949   }
 950   for (int i = 0; i < 6; i++) {
 951     serial_print_num(dolny_limit[i]);
 952     serial_print("-");
 953     serial_print_num(horny_limit[i]);
 954     serial_print(" ");
 955   }
 956   serial_println_flash(PSTR("ok"));
 957   pipni();
 958 }
 959 
 960 void vypis_kalibraciu()
 961 {
 962   serial_print_flash(PSTR("stredy: "));
 963   for (int i = 0; i < 6; i++) {
 964     serial_print_num(kalib[i]);
 965     serial_print(" ");
 966   }
 967   serial_println();
 968   
 969   serial_print_flash(PSTR("invertovane servo: "));
 970   for (int i = 0; i < 6; i++)
 971   {
 972     serial_print_num(servo_invertovane[i]);
 973     serial_print_char(' ');
 974   }
 975   serial_println();
 976     
 977   serial_print_flash(PSTR("dolny limit: "));
 978   for (int i = 0; i < 6; i++) {
 979     serial_print_num(dolny_limit[i]);
 980     serial_print(" ");
 981   }
 982   serial_println();
 983   serial_print_flash(PSTR("horny limit: "));
 984   for (int i = 0; i < 6; i++) {
 985     serial_print_num(horny_limit[i]);
 986     serial_print(" ");
 987   }
 988   serial_println();
 989 }
 990 
 991 void nacitaj_kalibraciu()
 992 {
 993   for (int i = 0; i < 6; i++)
 994     kalib[i] = nacitajCislo();
 995   vypis_kalibraciu();
 996   serial_println_flash(PSTR("ok"));
 997   pipni();
 998 }
 999 
1000 void zvys_krok()
1001 {
1002   if (krok < 180) krok++;
1003   serial_print_flash(PSTR("krok: "));
1004   serial_println_num(krok);
1005 }
1006 
1007 void zniz_krok()
1008 {
1009   if (krok > 0) krok--;
1010   serial_print_flash(PSTR("krok: "));
1011   serial_println_num(krok);
1012 }
1013 
1014 void zniz_spomalenie()
1015 {
1016   if (spomalenie > 0) spomalenie--;
1017   serial_print_flash(PSTR("spomalenie: "));
1018   serial_println_num(spomalenie);
1019 }
1020 
1021 void zvys_spomalenie()
1022 {
1023   if (spomalenie < 100) spomalenie++;
1024   serial_print_flash(PSTR("spomalenie: "));
1025   serial_println_num(spomalenie);
1026 }
1027 
1028 //nasleduju funkcie ktore pracuju s pamatom EEPROM
1029 
1030 //EEPROM MAP:
1031 // 0: slot number where choreography 3 starts (B3. Real address = B3 x 4)
1032 // 1: marker '~' indicates calibration is already stored in EEPROM and will be loaded on startup
1033 //           '1', '2', '3' indicate the same, but choreography 1,2, or 3 is automatically launched on startup
1034 // 2-8: servo calibration (central points to be used instead of default 90)
1035 //        they are shifted by (90-63) and the range is (90-63)...0  till (90+63)...127
1036 //        bit 7 (value 128) indicates servo is inverted
1037 // 9-14: lower limit for direct control for all 6 servos
1038 // 15-20: upper limit for direct control for all 6 servos
1039 // 21:    number of steps in choreography 1 (L1) 0=choreography not in memory
1040 // 22:    number of steps in choreography 2 (L2) 0=not in memory
1041 // 23:    number of steps in choreography 3 (L3) 0=not in memory
1042 // 24..(L1 x 4 + 23)       choreography1  tuplets (uint16,uint8,uint8) x L1
1043 // (1024 - L2 x 4)..1023   choreography2  same as above
1044 // B3 x 4..(B3 x 4 + L3 x 4 - 1)  choreography 3  same as above
1045 
1046 void skus_zapisat_choreografiu_do_EEPROM()
1047 {
1048   serial_print_flash(PSTR("Cislo? [1-3]: "));
1049   char odpoved = read_char();
1050   if ((odpoved < '1') || (odpoved > '3')) return;
1051   serial_println_char(odpoved);
1052   uint8_t cislo = odpoved - '0';
1053 
1054   serial_print_flash(PSTR("Zapisat choreografiu do EEPROM c."));
1055   serial_print_char(odpoved);
1056   serial_print_flash(PSTR("? [Y/n]: "));
1057   odpoved = read_char();
1058   serial_println_char(odpoved);
1059   if (odpoved == 'Y')
1060     zapis_choreografiu_do_EEPROM(cislo);
1061 }
1062 
1063 void skus_nacitat_choreografiu_z_EEPROM()
1064 {
1065   serial_print_flash(PSTR("Cislo? [1-3]: "));
1066   char odpoved = read_char();
1067   if ((odpoved < '1') || (odpoved > '3')) return;
1068   serial_println_char(odpoved);
1069   uint8_t cislo = odpoved - '0';
1070 
1071   serial_print_flash(PSTR("Precitat choreografiu z EEPROM c."));
1072   serial_print_char(odpoved);
1073   serial_print_flash(PSTR("? [Y/n]: "));
1074   odpoved = read_char();
1075   serial_println_char(odpoved);
1076   if (odpoved == 'Y')
1077     precitaj_choreografiu_z_EEPROM(cislo);
1078 }
1079 
1080 void skus_naformatovat_EEPROM_choreografie()
1081 {
1082   serial_print_flash(PSTR("Formatovat EEPROM choreografii? [Y/n]:"));
1083   char odpoved = read_char();
1084   serial_println_char(odpoved);
1085   if (odpoved == 'Y')
1086     naformatuj_EEPROM_choreografie();
1087 }
1088 
1089 void  zapis_choreografiu_do_EEPROM(int slot)
1090 {
1091   uint8_t b3 = EEPROM.read(0);
1092   uint8_t len1 = EEPROM.read(21);
1093   uint8_t len2 = EEPROM.read(22);
1094   uint8_t len3 = EEPROM.read(23);
1095   uint16_t wp = 65535;
1096 
1097   if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3  > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3  > 250 - len2) || (len1 + len2 > 250))
1098     b3 = len1 = len2 = len3 = 0;
1099 
1100   if (slot == 1)
1101   {
1102     if (((ch_len < b3) || (len3 == 0)) && (ch_len + len2 + len3 <= 250))
1103     {
1104       EEPROM.write(21, ch_len);
1105       wp = 24;
1106     }
1107   }
1108   else if (slot == 2)
1109   {
1110     if ((250 - b3 - len3 > ch_len) && (ch_len + len1 + len3 <= 250))
1111     {
1112       EEPROM.write(22, ch_len);
1113       wp = 1000 - ch_len * 4;
1114     }
1115   }
1116   else if (slot == 3)
1117   {
1118     if (ch_len + len1 + len2 <= 250)
1119     {
1120       EEPROM.write(23, ch_len);
1121       EEPROM.write(0, (len1 - len2) / 2 + 125);
1122       wp = 4 * ((len1 - len2) / 2 + 125);
1123     }
1124   }
1125 
1126   if (wp == 65535)
1127     serial_println_flash(PSTR("not enough space"));
1128   else
1129   {
1130     for (int i = 0; i < ch_len; i++)
1131     {
1132       EEPROM.write(wp + 4 * i, ch_time[i] & 255);
1133       EEPROM.write(wp + 1 + 4 * i, ch_time[i] >> 8);
1134       EEPROM.write(wp + 2 + 4 * i, ch_servo[i]);
1135       EEPROM.write(wp + 3 + 4 * i, ch_val[i]);
1136     }
1137     serial_println_flash(PSTR("ok"));
1138   }
1139 }
1140 
1141 void precitaj_choreografiu_z_EEPROM(uint8_t slot)
1142 {
1143   uint8_t b3 = EEPROM.read(0);
1144   uint8_t len1 = EEPROM.read(21);
1145   uint8_t len2 = EEPROM.read(22);
1146   uint8_t len3 = EEPROM.read(23);
1147   uint16_t rp = 65535;
1148   if ((len1 > CHOREO_LEN) || (len2 > CHOREO_LEN) || (len3 > CHOREO_LEN) || (b3  > 250 - len3) || ((len1 > b3) && (b3 != 0)) || (len3 + b3  > 250 - len2) || (len1 + len2 > 250))
1149     b3 = len1 = len2 = len3 = 0;
1150 
1151   if (slot == 1)
1152   {
1153     if (len1 > 0)
1154     {
1155       rp = 24;
1156       ch_len = len1;
1157     }
1158   }
1159   else if (slot == 2)
1160   {
1161     if (len2 > 0)
1162     {
1163       ch_len = len2;
1164       rp = 1000 - ch_len * 4;
1165     }
1166   }
1167   else if (slot == 3)
1168   {
1169     if (len3 > 0)
1170     {
1171       rp = b3 * 4;
1172       ch_len = len3;
1173     }
1174   }
1175 
1176   if (rp == 65535)
1177     serial_println_flash(PSTR("couldn't"));
1178   else
1179   {
1180     for (int i = 0; i < ch_len; i++)
1181     {
1182       ch_time[i] = ((uint16_t)EEPROM.read(rp + 4 * i)) |
1183                    (((uint16_t)EEPROM.read(rp + 1 + 4 * i)) << 8);
1184       ch_servo[i] = EEPROM.read(rp + 2 + 4 * i);
1185       ch_val[i] = EEPROM.read(rp + 3 + 4 * i);
1186     }
1187     serial_println_flash(PSTR("ok"));
1188     pipni();
1189   }
1190 }
1191 
1192 void naformatuj_EEPROM_choreografie()
1193 {
1194   EEPROM.write(0, 0);
1195   EEPROM.write(21, 0);
1196   EEPROM.write(22, 0);
1197   EEPROM.write(23, 0);
1198   serial_println_flash(PSTR("ok"));
1199   pipni();
1200 }
1201 
1202 void precitaj_kalibraciu_z_EEPROM()
1203 {
1204   uint8_t value = EEPROM.read(1);
1205   if ((value != '~') && (value != '1') && (value != '2') && (value != '3')) return;
1206   if (value != '~') auto_start = value - '0';
1207   else auto_start = 0;
1208   for (int i = 2; i < 8; i++)
1209   {
1210     int16_t k = EEPROM.read(i);
1211     if (k > 127) servo_invertovane[i - 2] = 1;
1212     else servo_invertovane[i - 2] = 0;
1213     k &= 127;
1214     prednastavena_kalibracia[i - 2] = k + (90-63);
1215   }
1216   for (int i = 0; i < 6; i++)
1217     dolny_limit[i] = EEPROM.read(i + 9);
1218   for (int i = 0; i < 6; i++)
1219     horny_limit[i] = EEPROM.read(i + 15);
1220 }
1221 
1222 void zapis_kalibraciu_do_EEPROM()
1223 {
1224   serial_print_flash(PSTR("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: "));
1225   char odpoved = read_char();
1226   serial_println_char(odpoved);
1227   if (odpoved == 'Y')
1228   {
1229     char kalib_state = EEPROM.read(1);
1230     if ((kalib_state == '~') || ((kalib_state >= '1') && (kalib_state <= '3')))
1231       EEPROM.write(1, kalib_state);
1232     else EEPROM.write(1, '~');
1233     for (int i = 2; i < 8; i++)
1234     {
1235       int16_t k = kalib[i - 2] - (90-63);
1236       if (k < 0) k = 0;
1237       else if (k > 127) k = 127;
1238       if (servo_invertovane[i - 2]) k += 128;      
1239       EEPROM.write(i, (uint8_t)k);
1240     }
1241     for (int i = 0; i < 6; i++)
1242       EEPROM.write(9 + i, dolny_limit[i]);
1243     for (int i = 0; i < 6; i++)
1244       EEPROM.write(15 + i, horny_limit[i]);
1245     serial_println_flash(PSTR("ok"));
1246   }
1247 }
1248 
1249 // nasleduje softverova implementacia serioveho portu
1250 #define SERIAL_STATE_IDLE      0
1251 #define SERIAL_STATE_RECEIVING 1
1252 #define SERIAL_BUFFER_LENGTH   20
1253 
1254 static volatile uint8_t serial_state;
1255 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
1256 static volatile uint8_t serial_buf_wp, serial_buf_rp;
1257 
1258 static volatile uint8_t receiving_byte;
1259 
1260 static volatile uint32_t time_startbit_noticed;
1261 static volatile uint8_t next_bit_order;
1262 static volatile uint8_t waiting_stop_bit;
1263 static uint16_t one_byte_duration;
1264 static uint16_t one_bit_duration;
1265 static uint16_t one_bit_write_duration;
1266 static uint16_t half_of_one_bit_duration;
1267 
1268 void init_serial(uint32_t baud_rate)
1269 {
1270   pinMode(2, INPUT);
1271   pinMode(4, OUTPUT);
1272 
1273   serial_state = SERIAL_STATE_IDLE;
1274 
1275   one_byte_duration = 9500000 / baud_rate;
1276   one_bit_duration = 1000000 / baud_rate;
1277   one_bit_write_duration = one_bit_duration - 1;
1278   half_of_one_bit_duration = 500000 / baud_rate;
1279 
1280   PCMSK2 |= 4; //PCINT18;
1281   PCIFR &= ~4; //PCIF2;
1282   PCICR |= 4; // PCIE2;
1283 }
1284 
1285 ISR(PCINT2_vect)
1286 {
1287   uint32_t tm = micros();
1288   if (serial_state == SERIAL_STATE_IDLE)
1289   {
1290     time_startbit_noticed = tm;
1291     serial_state = SERIAL_STATE_RECEIVING;
1292     receiving_byte = 0xFF;
1293     next_bit_order = 0;
1294   }
1295   else if (tm - time_startbit_noticed > one_byte_duration)
1296   {
1297     serial_buffer[serial_buf_wp] = receiving_byte;
1298     serial_buf_wp++;
1299     if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1300     time_startbit_noticed = tm;
1301     receiving_byte = 0xFF;
1302     next_bit_order = 0;
1303   }
1304   else if (PIND & 4)
1305   {
1306     int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1307     while (next_bit_order < new_next_bit_order)
1308     {
1309       receiving_byte &= ~(1 << next_bit_order);
1310       next_bit_order++;
1311     }
1312     if (next_bit_order == 8)
1313     {
1314       serial_buffer[serial_buf_wp] = receiving_byte;
1315       serial_buf_wp++;
1316       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1317       serial_state = SERIAL_STATE_IDLE;
1318     }
1319   } else
1320     next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1321 }
1322 
1323 uint8_t serial_available()
1324 {
1325   cli();
1326   if (serial_buf_rp != serial_buf_wp)
1327   {
1328     sei();
1329     return 1;
1330   }
1331   if (serial_state == SERIAL_STATE_RECEIVING)
1332   {
1333     uint32_t tm = micros();
1334     if (tm - time_startbit_noticed > one_byte_duration)
1335     {
1336       serial_state = SERIAL_STATE_IDLE;
1337       serial_buffer[serial_buf_wp] = receiving_byte;
1338       serial_buf_wp++;
1339       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1340       sei();
1341       return 1;
1342     }
1343   }
1344   sei();
1345   return 0;
1346 }
1347 
1348 int16_t serial_read()
1349 {
1350   cli();
1351   if (serial_buf_rp != serial_buf_wp)
1352   {
1353     uint8_t ch = serial_buffer[serial_buf_rp];
1354     serial_buf_rp++;
1355     if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
1356     sei();
1357     return ch;
1358   }
1359 
1360   if (serial_state == SERIAL_STATE_RECEIVING)
1361   {
1362     uint32_t tm = micros();
1363     if (tm - time_startbit_noticed > one_byte_duration)
1364     {
1365       uint8_t ch = receiving_byte;
1366       serial_state = SERIAL_STATE_IDLE;
1367       sei();
1368       return ch;
1369     }
1370   }
1371   sei();
1372   return -1;
1373 }
1374 
1375 void serial_write(uint8_t ch)
1376 {
1377 #ifdef ECHO_BT_TO_USB
1378   Serial.print((char)ch);
1379 #endif
1380   PORTD &= ~16;
1381   delayMicroseconds(one_bit_write_duration);
1382   for (uint8_t i = 0; i < 8; i++)
1383   {
1384     if (ch & 1) PORTD |= 16;
1385     else PORTD &= ~16;
1386     ch >>= 1;
1387     delayMicroseconds(one_bit_write_duration);
1388   }
1389   PORTD |= 16;
1390   delayMicroseconds(one_bit_write_duration);
1391   delayMicroseconds(one_bit_write_duration);
1392   delayMicroseconds(one_bit_write_duration);
1393   delayMicroseconds(one_bit_write_duration);
1394   delayMicroseconds(one_bit_write_duration);
1395 }
1396 
1397 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
1398 {
1399   uint16_t len;
1400   int16_t ch;
1401   do {
1402     ch = serial_read();
1403     if (ch == 13) continue;
1404   } while (ch == -1);
1405 
1406   do {
1407     if ((ch != 13) && (ch != 10) && (ch != -1))
1408     {
1409       *(ln++) = ch;
1410       max_length--;
1411       len++;
1412     }
1413     ch = serial_read();
1414   } while ((ch != 13) && max_length);
1415   *ln = 0;
1416   return len;
1417 }
1418 
1419 void serial_print_num(int32_t number)
1420 {
1421   if (number < 0)
1422   {
1423     serial_write('-');
1424     number = -number;
1425   }
1426   int32_t rad = 1;
1427   while (number / rad) rad *= 10;
1428   if (number > 0) rad /= 10;
1429   while (rad)
1430   {
1431     serial_write((char)('0' + (number / rad)));
1432     number -= (number / rad) * rad;
1433     rad /= 10;
1434   }
1435 }
1436 
1437 void serial_print_char(char ch)
1438 {
1439   serial_write(ch);
1440 }
1441 
1442 void serial_print(const char *str)
1443 {
1444   while (*str) serial_write(*(str++));
1445 }
1446 
1447 void serial_println(const char *str)
1448 {
1449   serial_print(str);
1450   serial_write(13);
1451   serial_write(10);
1452 }
1453 
1454 void serial_print_flash(const char *str)
1455 {
1456   int ln = strlen_P(str);
1457   for (int i = 0; i < ln; i++)
1458     serial_write(pgm_read_byte(str + i));
1459 }
1460 
1461 void serial_println_flash(const char *str)
1462 {
1463   serial_print_flash(str);
1464   serial_write(13);
1465   serial_write(10);
1466 }
1467 
1468 void serial_println_num(int32_t number)
1469 {
1470   serial_print_num(number);
1471   serial_println();
1472 }
1473 
1474 void serial_println_char(char ch)
1475 {
1476   serial_write(ch);
1477   serial_println();
1478 }
1479 
1480 void serial_println()
1481 {
1482   serial_write(13);
1483   serial_write(10);
1484 }
1485 
1486 // nasleduje citanie z utltazvukoveho senzora
1487 
1488 static volatile uint32_t pulse_start;
1489 static volatile uint8_t new_distance;
1490 
1491 void init_ultrasonic()
1492 {
1493   pinMode(US_ECHO, INPUT);
1494   pinMode(US_TRIG, OUTPUT);
1495 
1496   PCMSK0 |= 1; //PCINT0;
1497   PCIFR &= ~1; //PCIF0;
1498   PCICR |= 1; // PCIE0;
1499 }
1500 
1501 ISR(PCINT0_vect)
1502 {
1503   if (PINB & 1) pulse_start = micros();
1504   else
1505   {
1506     distance = (int16_t)((micros() - pulse_start) / 58);
1507     new_distance = 1;
1508   }
1509 }
1510 
1511 void start_distance_measurement()
1512 {
1513   distance = 10000;
1514   new_distance = 0;
1515   digitalWrite(US_TRIG, HIGH);
1516   delayMicroseconds(10);
1517   digitalWrite(US_TRIG, LOW);
1518 }
1519 
1520 void wait_for_distance_measurement_to_complete()
1521 {
1522   uint8_t counter = 0;
1523   while ((counter < 20) && !new_distance)
1524   {
1525     delay(1);
1526     counter++;
1527   }
1528   if (counter == 20)
1529   {
1530     pinMode(US_ECHO, OUTPUT);
1531     digitalWrite(US_ECHO, HIGH);
1532     delayMicroseconds(10);
1533     digitalWrite(US_ECHO, LOW);
1534     pinMode(US_ECHO, INPUT);
1535     delayMicroseconds(5);
1536     distance = 10000;
1537   }
1538 }
1539 
1540 int16_t measure_distance()
1541 {
1542   start_distance_measurement();
1543   wait_for_distance_measurement_to_complete();
1544   return distance;
1545 }
1546 
1547 //-------------------------------- nasleduje prehravanie melodie a hranie cez timer2 v pozadi
1548 #define SIRENE_PORT  PORTB
1549 #define SIRENE_DDR   DDRB
1550 #define SIRENE_PIN   4
1551 
1552 #define FIS3 2960
1553 #define G3 3136
1554 
1555 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 };
1556 
1557 //popcorn
1558 uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217, 36};
1559 const uint8_t melodia1[] PROGMEM = { 252, 50, 149,  49,
1560                                      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,
1561                                      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,
1562                                      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,
1563                                      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,
1564                                      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,
1565                                      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,
1566                                      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,
1567                                      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 ,
1568                                      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,
1569                                      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,
1570                                      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 ,
1571                                      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
1572                                    };
1573 
1574 //kohutik jarabi
1575 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 };
1576 
1577 //kankan
1578 const uint8_t melodia3[] PROGMEM = { 252, 100,
1579                                      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,
1580                                      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,
1581                                      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,
1582                                      135, 131, 128, 126, 75, 76, 78, 80, 81, 76, 80, 76, 81, 76, 80, 76, 81, 76, 80, 76, 81, 76, 80, 76,
1583                                      251, 2, 11, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1584                                      251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1585                                      251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1586                                      251, 1, 4, 3, 16, 19, 251, 1, 4, 3, 16, 19,
1587                                      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,
1588                                      174, 88, 91, 90, 88, 143, 143, 93, 95, 90, 91, 138, 138, 88, 91, 90, 88, 86, 93, 89, 90, 136,
1589                                      64, 71, 71, 73, 71, 69, 69, 73, 74, 78, 81, 78, 78, 76, 251, 1, 184, 1, 32, 126,
1590                                      78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126, 78, 76, 126,
1591                                      78, 76, 78, 76, 78, 76, 78, 76,
1592                                      131, 119, 119, 119, 169
1593                                    };
1594 
1595 //labutie jazero
1596 const uint8_t melodia4[] PROGMEM = {
1597   252, 220, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69,
1598   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,
1599   99, 83, 81, 80,
1600   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,
1601   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,
1602   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,
1603   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,
1604   99, 83, 81, 80,
1605   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
1606 };
1607 
1608 //let it be https://www.musicnotes.com/sheetmusic/mtd.asp?ppn=MN0101556
1609 const uint8_t melodia5[] PROGMEM = { 252, 200, 26, 26, 76, 26, 253, 78, 73, 76, 76, 31, 253, 83, 35, 85, 253, 85, 
1610                                      83, 83, 81, 131, 35, 253, 85, 86, 35, 35, 85, 83, 99, 35, 33, 83, 81, 181 
1611 };
1612 
1613 volatile int16_t music_speed = 800 / 16;
1614 volatile const uint8_t *current_note ;
1615 volatile uint16_t notes_remaining;
1616 uint8_t dotted_note = 0;
1617 
1618 void zahraj_melodiu(uint8_t cislo)
1619 {
1620   if (cislo == 0) {
1621     zastav_melodiu();
1622     return;
1623   }
1624 
1625   if (cislo == 1) current_note = melodia1;
1626   else if (cislo == 2) current_note = melodia2;
1627   else if (cislo == 3) current_note = melodia3;
1628   else if (cislo == 4) current_note = melodia4;
1629   else if (cislo == 5) current_note = melodia5;
1630   notes_remaining = dlzka_melodia[cislo];
1631 
1632   next_note();
1633 }
1634 
1635 void next_note()
1636 {
1637   uint16_t freq = 0, dur = 0;
1638   if (!notes_remaining) return;
1639   otto_translate_tone_flash(&freq, &dur);
1640   tone2(freq, dur);
1641 }
1642 
1643 void otto_translate_tone_flash(uint16_t *freq, uint16_t *del)
1644 {
1645   do {
1646     uint8_t n = pgm_read_byte(current_note);
1647     if (n > 249)
1648     {
1649       if (n == 251)
1650       {
1651         current_note++;
1652         uint8_t f1 = pgm_read_byte(current_note);
1653         current_note++;
1654         uint8_t f2 = pgm_read_byte(current_note);
1655         current_note++;
1656         uint8_t d1 = pgm_read_byte(current_note);
1657         current_note++;
1658         uint8_t d2 = pgm_read_byte(current_note);
1659         *freq = (f1 << 8) + f2;
1660         *del = (music_speed * 16 * (long)d1) / d2;
1661         notes_remaining -= 4;
1662       }
1663       else if (n == 252)
1664       {
1665         current_note++;
1666         music_speed = pgm_read_byte(current_note);
1667         current_note++;
1668         notes_remaining -= 2;
1669         continue;
1670       }
1671       else if (n == 253) 
1672       {
1673         dotted_note = 1;
1674         current_note++;
1675         notes_remaining--;        
1676         continue;
1677       }
1678       else if (n == 254)
1679       {
1680         *freq = FIS3;
1681         *del = music_speed;
1682       }
1683       else if (n == 255)
1684       {
1685         *freq = G3;
1686         *del = music_speed;
1687       }
1688     }
1689     else
1690     {
1691       uint8_t len = n / 50;
1692       *del = music_speed;
1693       while (len--) *del *= 2;
1694       n = n % 50;
1695       if (n != 49)
1696       {
1697         uint8_t octave = (n + 5) / 12;
1698         n = (n + 5) % 12;
1699         float ffreq = octave_4[n];
1700         octave = 4 - octave;
1701         while (octave > 0)
1702         {
1703           ffreq /= 2.0;
1704           octave--;
1705         }
1706         *freq = (uint16_t)ffreq;
1707       }
1708       else *freq = 0;
1709     }
1710     notes_remaining--;
1711     current_note++;
1712     break;
1713   } while (1);
1714   if (dotted_note) 
1715   {
1716     *del += (*del) / 2;
1717     dotted_note = 0;
1718   }
1719 }
1720 
1721 static volatile uint8_t tone2_state;
1722 static volatile uint8_t tone2_pause;
1723 static volatile uint32_t tone2_len;
1724 
1725 void init_tone2()
1726 {
1727   notes_remaining = 0;
1728   tone2_pause = 0;
1729   dotted_note = 0;
1730   TCCR2A = 2;
1731   TCCR2B = 0;
1732   TIMSK2 = 2;
1733   SIRENE_DDR |= (1 << SIRENE_PIN);
1734 }
1735 
1736 ISR(TIMER2_COMPA_vect)
1737 {
1738   if (!tone2_pause)
1739   {
1740     if (tone2_state)
1741     {
1742       SIRENE_PORT |= (1 << SIRENE_PIN);
1743       tone2_state = 0;
1744     }
1745     else
1746     {
1747       SIRENE_PORT &= ~(1 << SIRENE_PIN);
1748       tone2_state = 1;
1749     }
1750   }
1751   if ((--tone2_len) == 0)
1752   {
1753     TCCR2B = 0;
1754     tone2_pause = 0;
1755     next_note();
1756   }
1757 }
1758 
1759 void tone2(uint16_t freq, uint16_t duration)
1760 {
1761   uint32_t period = ((uint32_t)1000000) / (uint32_t)freq;
1762 
1763   if (freq >= 977)  // prescaler 32
1764   {
1765     tone2_state = 0;
1766     tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1767     if (tone2_len == 0) tone2_len++;
1768     TCNT2 = 0;
1769     OCR2A = (uint8_t) (250000 / (uint32_t)freq);
1770     TCCR2B = 3;
1771   }
1772   else if (freq >= 488) // prescaler 64
1773   {
1774     tone2_state = 0;
1775     tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1776     if (tone2_len == 0) tone2_len++;
1777     TCNT2 = 0;
1778     OCR2A = (uint8_t) (125000 / (uint32_t)freq);
1779     TCCR2B = 4;
1780   }
1781   else if (freq >= 244) // prescaler 128
1782   {
1783     tone2_state = 0;
1784     tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1785     if (tone2_len == 0) tone2_len++;
1786     TCNT2 = 0;
1787     OCR2A = (uint8_t) (62500 / (uint32_t)freq);
1788     TCCR2B = 5;
1789   }
1790   else if (freq >= 122) //prescaler 256
1791   {
1792     tone2_state = 0;
1793     tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1794     if (tone2_len == 0) tone2_len++;
1795     TCNT2 = 0;
1796     OCR2A = (uint8_t) (31250 / (uint32_t)freq);
1797     TCCR2B = 6;
1798   }
1799   else if (freq >= 30) //prescaler 1024
1800   {
1801     tone2_state = 0;
1802     tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1803     if (tone2_len == 0) tone2_len++;
1804     TCNT2 = 0;
1805     OCR2A = (uint8_t) (7813 / (uint32_t)freq);
1806     TCCR2B = 7;
1807   }
1808   else if (freq == 0)
1809   {
1810     tone2_pause = 1;
1811     tone2_state = 0;
1812     period = 1000000 / 500;
1813     tone2_len = ((uint32_t)duration * (uint32_t)1000) * 2 / period;
1814     TCNT2 = 0;
1815     OCR2A = (uint8_t) (125000 / (uint32_t)500);
1816     TCCR2B = 4;
1817   }
1818   else
1819   {
1820     TCCR2B = 0;
1821   }
1822 }
1823 
1824 void zastav_melodiu()
1825 {
1826   notes_remaining = 0;
1827 }