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

From DT^2
Jump to: navigation, search
m
m
Line 1,654: Line 1,654:
 
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 };
 
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 };
  
// kankan
+
//kankan
 
const uint8_t melodia3[] PROGMEM = { 252, 100,
 
const uint8_t melodia3[] PROGMEM = { 252, 100,
 
                                     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,
 
                                     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,

Revision as of 09:53, 12 August 2018

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

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

Pripojenie na vývody Arduina:

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