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

From DT^2
Jump to: navigation, search
m
m
Line 348: Line 348:
 
   else if (z == '/') zahraj_melodiu(3);
 
   else if (z == '/') zahraj_melodiu(3);
 
   else if (z == '+') zahraj_melodiu(4);
 
   else if (z == '+') zahraj_melodiu(4);
 +
  else if (z == '!') zahraj_melodiu(5);
 
   else if (z == 27) zastav_melodiu();
 
   else if (z == 27) zastav_melodiu();
 
   else return 0;
 
   else return 0;
Line 1,679: Line 1,680:
  
 
//popcorn
 
//popcorn
uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217};
+
uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217, 21};
 
const uint8_t melodia1[] PROGMEM = { 252, 50, 149,  49,
 
const uint8_t melodia1[] PROGMEM = { 252, 50, 149,  49,
 
                                     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,
 
                                     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,
Line 1,728: Line 1,729:
 
   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
 
   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
 
};
 
};
 +
 +
//let it be
 +
const uint8_t melodia5[] PROGMEM = { 252,200,26,26,76,26,28,78,73,76,76,31,83,35,85,35,85,83,83,81,131 };
  
 
volatile int16_t music_speed = 800 / 16;
 
volatile int16_t music_speed = 800 / 16;
Line 1,746: Line 1,750:
 
   else if (cislo == 3) current_note = melodia3;
 
   else if (cislo == 3) current_note = melodia3;
 
   else if (cislo == 4) current_note = melodia4;
 
   else if (cislo == 4) current_note = melodia4;
 +
  else if (cislo == 5) current_note = melodia5;
 
   notes_remaining = dlzka_melodia[cislo];
 
   notes_remaining = dlzka_melodia[cislo];
  

Revision as of 09:22, 31 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:

pin signál
10 ľavá ruka
11 pravá ruka
9 ľavá noha
6 pravá noha
5 ľavá päta
3 pravá päta
2 TXD BlueTooth
4 RXD BlueTooth
7 TRIG Ultrazvuk
8 ECHO Ultrazvuk
12 Siréna +
G Siréna -, GND Ultrazvuk, GND BlueTooth, kondenzátor -, najtmavší káblik každého serva, oba kábliky z vypínača
V VCC Ultrazvuk, VCC BlueTooth, kondenzátor +, červený káblik každého serva, oba kabliky z diody

1 HC-SR04 Front Back.jpeg

Číslovanie serva v choreografii:

servo číslo serva v choreografii
ľavá ruka 5
pravá ruka 6
ľavá noha 4
pravá noha 3
ľavá päta 2
pravá päta 1
klaves servo/funkcia
a/q ľavá ruka
;/p pravá ruka
z/x ľavá noha
,/. pravá noha
d/c ľavá päta
k/m pravá päta
1/9 zníž/zvýš rýchlosť pohybu
H kalibrovať strednú polohu servomotorov
  • pomocou + a - nájdeme strednú polohu serva
  • enter potvrdí hodnotu a pokračuje sa ďalším servom
  • kalibrácia sa po vypnutí nezapamätá, pokiaľ ju neuložíte do EEPROM (pozri nižšie)
J kalibrovať limity pre všetky stupne voľnosti (len pre priame riadenie)
G zobrazenie celej kalibrácie
E celú kalibráciu je možné zapísať do trvalej pamäte EEPROM (používať opatrne! - dá sa prepísať najviac 100000-krát), po zapnutí sa automaticky načíta, treba potvrdiť veľkým Y
minus zvukový pozdrav
R ruky hore-dole
@ načítanie choreografie, pozri príklady a formát
? zobrazí načítanú choreografiu (kontrola po @)
t zatancuje načítanú choreografiu
U testuje ultrazvuk
S zobrazí aktuálnu polohu servomotorov (vhodné pri ladení choreografie)
medzera reset všetkých motorov do strednej polohy
3,...,6 zmiešaný pohyb (treba nastaviť vo funkciách kombinacia1() - kombinacia4())
v,b,n,j,h,g,f,s,l,o,i,u,y,r,e,w zvukové efekty 1-16
TAB,*,/,+ zahrá melódiu 1-4
W zapísať choreografiu do EEPROM (opatrne)
C načítať choreografiu z EEPROM
X formátovať EEPROM na zápis choreografií (opatrne)
V vypni/zapni režim nudy
A automatický štart choreografie po zapnutí

Pozrite si (a skopírujte si) príklady choreografií: Otto - príklady choreografií

Download: otto4.ino

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