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

From DT^2
Jump to: navigation, search
m
m
Line 133: Line 133:
 
#define CHOREO_LEN 200
 
#define CHOREO_LEN 200
  
#define CAS_OTTO_SA_ZACNE_NUDIT 12000
+
#define CAS_OTTO_SA_ZACNE_NUDIT 9000
  
 
// cisla pinov, kde su zapojene servo motory
 
// cisla pinov, kde su zapojene servo motory
Line 599: Line 599:
 
{
 
{
 
   if (cmd == 1) vpred();
 
   if (cmd == 1) vpred();
   if (cmd == 2) zahraj_melodiu(1);
+
   if (cmd == 2) { precitaj_choreografiu_z_EEPROM(1); zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len); }
   if (cmd == 3) melodia_jedna_druhej();
+
   if (cmd == 3) { precitaj_choreografiu_z_EEPROM(2); zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len); }
   if (cmd == 4) ahoj();
+
   if (cmd == 4) { precitaj_choreografiu_z_EEPROM(3); zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len); }
   if (cmd == 5) zahraj_melodiu(2);
+
   if (cmd == 5) zahraj_melodiu(1);
   if (cmd == 6) zahraj_melodiu(3);
+
   if (cmd == 6) melodia_jedna_druhej();
   if (cmd == 7) zahraj_melodiu(4);
+
  if (cmd == 7) ahoj();
   serial_println_num(cmd);
+
  if (cmd == 9) zahraj_melodiu(2);
 +
   if (cmd == 9) zahraj_melodiu(3);
 +
   if (cmd == 10) zahraj_melodiu(4);
 
}
 
}
  

Revision as of 10:03, 10 August 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:

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)
V vypni/zapni režim nudy

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

Download: otto4.ino

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