Otto - riadiaci program v.4

From DT^2
Revision as of 20:09, 16 August 2018 by Admin (talk | contribs)
Jump to: navigation, search

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

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

Pripojenie na vývody Arduina:

signál pin
ľavá ruka 10
pravá ruka 11
ľavá noha 9
pravá noha 6
ľavá päta 5
pravá päta 3
TXD BlueTooth 2
RXD BlueTooth 4
TRIG Ultrazvuk 7
ECHO Ultrazvuk 8
Siréna + 12

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