Otto - riadiaci program v.4

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