Otto - riadiaci program v.4

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

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