Otto - riadiaci program v.4

From DT^2
Revision as of 19:34, 25 August 2018 by Palo (talk | contribs)
Jump to: navigation, search

Program umožňuje priame riadenie robota (aj cez BlueTooth). Teraz už funguje aj na pinoch 2,4 a môže sa naraz programovať aj komunikovať.

Na komunikáciu s robotom môžete použiť napr. program Putty: putty.exe.

Pripojenie na vývody Arduina:

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