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

From DT^2
Jump to: navigation, search
m
m
Line 77: Line 77:
 
|G || zobrazenie celej kalibrácie
 
|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
+
|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
 
|minus || zvukový pozdrav
Line 99: Line 99:
 
|v,b,n,j,h,g,f,s,l,o,i,u,y,r,e,w || zvukové efekty 1-16
 
|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)
 +
|-
 +
|}
  
 
Pozrite si (a skopírujte si) príklady choreografií: [[Otto - príklady choreografií]]
 
Pozrite si (a skopírujte si) príklady choreografií: [[Otto - príklady choreografií]]

Revision as of 04:50, 10 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:

servo 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

Čí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
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)

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

Download: otto3.ino

   1 #include <Servo.h>
   2 #include <EEPROM.h>
   3 #include <avr/pgmspace.h>
   4 
   5 #define ECHO_BT_TO_USB 1
   6 
   7 #define US_TRIG  7
   8 #define US_ECHO  8
   9 
  10 #define BT_RX   2 
  11 #define BT_TX   4
  12 
  13 #define SIRENA 12
  14 
  15 //maximalna dlzka choreografie
  16 #define CHOREO_LEN 200
  17 
  18 // cisla pinov, kde su zapojene servo motory
  19 #define PIN_SERVO_LAVA_RUKA   10 
  20 #define PIN_SERVO_PRAVA_RUKA  11
  21 #define PIN_SERVO_LAVA_NOHA   9
  22 #define PIN_SERVO_PRAVA_NOHA  6
  23 #define PIN_SERVO_LAVA_PATA   5
  24 #define PIN_SERVO_PRAVA_PATA  3
  25 
  26 #define S1 3
  27 #define S2 5
  28 #define S3 6
  29 #define S4 9
  30 #define S5 10
  31 #define S6 11
  32 #define SIRENA 12
  33 
  34 //maximalna dlzka choreografie
  35 #define CHOREO_LEN 200
  36 
  37 // tu su serva cislovane 1-6
  38 #define SERVO_LAVA_RUKA   5 
  39 #define SERVO_PRAVA_RUKA  6
  40 #define SERVO_LAVA_NOHA   4
  41 #define SERVO_PRAVA_NOHA  3
  42 #define SERVO_LAVA_PATA   2
  43 #define SERVO_PRAVA_PATA  1
  44 
  45 // ak su niektore serva naopak, je tu jednotka
  46 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
  47 
  48 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
  49 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
  50 // co robia jednotlive znaky (znamienko urcuje smer)
  51 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA, 
  52                   SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA, 
  53                   -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA, 
  54                   -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA, 
  55                    SERVO_LAVA_PATA, -SERVO_LAVA_PATA, 
  56                    SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA }; 
  57 
  58 // sem si mozno ulozit svoju kalibraciu
  59 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
  60 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
  61 
  62 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
  63 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
  64 
  65 Servo s[6];
  66 
  67 uint16_t ch_time[CHOREO_LEN];
  68 uint8_t ch_servo[CHOREO_LEN];
  69 uint8_t ch_val[CHOREO_LEN];
  70 int ch_len;
  71 uint8_t kalib[6];
  72 int stav[6];
  73 int krok;
  74 uint8_t spomalenie;
  75 
  76 static volatile int16_t distance;
  77 
  78 #define FIS3 2960
  79 #define G3 3136
  80 
  81 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 };
  82 int16_t music_speed;
  83 
  84 //popcorn
  85 uint16_t dlzka_melodia[] = {0, 386, 26, 281, 217};
  86 const uint8_t melodia1[] PROGMEM = { 252, 50, 149,  49, 
  87 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,
  88 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,
  89 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, 
  90 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,
  91 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,
  92 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,
  93 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, 
  94 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 ,
  95 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, 
  96 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, 
  97 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 ,
  98 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
  99 };
 100 
 101 //kohutik jarabi
 102 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 };
 103 
 104 // kankán
 105 const uint8_t melodia3[] PROGMEM = { 252, 100,
 106 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,
 107 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,
 108 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,
 109 135,131,128,126,75,76,78,80,81,76,80,76,81,76,80,76,81,76,80,76,81,76,80,76,
 110 251,2,11,3,16,19,251,1,4,3,16,19,
 111 251,1,4,3,16,19,251,1,4,3,16,19,
 112 251,1,4,3,16,19,251,1,4,3,16,19,
 113 251,1,4,3,16,19,251,1,4,3,16,19,
 114 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,
 115 174,88,91,90,88,143,143,93,95,90,91,138,138,88,91,90,88,86,93,89,90,136,
 116 64,71,71,73,71,69,69,73,74,78,81,78,78,76,251,1,184,1,32,126,
 117 78,76,126,78,76,126,78,76,126,78,76,126,78,76,126,78,76,126,
 118 78,76,78,76,78,76,78,76,
 119 131,119,119,119,169 };
 120 
 121 //labutie jazero
 122 const uint8_t melodia4[] PROGMEM = {
 123 252, 220, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69, 66, 69, 73, 69,
 124 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,
 125 99, 83, 81, 80, 
 126 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,
 127 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, 
 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, 73, 3, 8, 86, 133, 86, 91, 251, 7, 184, 3, 8, 87, 251, 7, 184, 3, 8, 85,
 129 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,
 130 99, 83, 81, 80, 
 131 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 };
 132 
 133 void setup() {
 134   Serial.begin(9600);
 135   init_serial(9600);
 136   init_ultrasonic();
 137 
 138   randomSeed(analogRead(1));
 139   s[0].attach(PIN_SERVO_PRAVA_PATA);
 140   s[1].attach(PIN_SERVO_LAVA_PATA);
 141   s[2].attach(PIN_SERVO_PRAVA_NOHA);
 142   s[3].attach(PIN_SERVO_LAVA_NOHA);
 143   s[4].attach(PIN_SERVO_LAVA_RUKA);
 144   s[5].attach(PIN_SERVO_PRAVA_RUKA);
 145   precitaj_kalibraciu_z_EEPROM();
 146   for (int i = 0; i < 6; i++)
 147   {
 148     kalib[i] = prednastavena_kalibracia[i];
 149     stav[i] = kalib[i];
 150     s[i].write(stav[i]);
 151   }
 152   ch_len = 0;
 153   krok = 7;
 154   spomalenie = 6;
 155   ahoj();
 156   ruky2();
 157   delay(100);  
 158   serial_println("\r\n  Otto DTDT");
 159 }
 160 
 161 void loop() {
 162   char z = -1;
 163   if (serial_available()) z = serial_read();
 164 #ifdef ECHO_BT_TO_USB
 165     if (Serial.available()) z = Serial.read();
 166 #endif  
 167   
 168   if (z != -1)
 169   { 
 170     if (pohyb_znakom(z)) return;   
 171     else if (pohyb_kombinacia(z)) return; 
 172     else if (vydaj_zvukovy_efekt(z)) return;
 173     else if (ma_zahrat_melodiu(z)) return;
 174     else if (z == '@') nacitaj_choreografiu();
 175     else if (z == '?') vypis_choreografiu();
 176     else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
 177     else if (z == '-') ahoj();
 178     else if (z == ' ') reset();
 179     else if (z == 'H') kalibruj();
 180     else if (z == 'J') nastav_limity();
 181     else if (z == 'G') vypis_kalibraciu();
 182     else if (z == 'L') nacitaj_kalibraciu();
 183     else if (z == 'E') zapis_kalibraciu_do_EEPROM();
 184     else if (z == 'R') ruky();
 185     else if (z == '9') zvys_krok();
 186     else if (z == '1') zniz_krok();
 187     else if (z == '8') zvys_spomalenie();
 188     else if (z == '7') zniz_spomalenie();
 189     else if (z == 'U') test_ultrazvuk();
 190     else if (z == 'S') zobraz_stav();
 191     
 192   }
 193   int16_t d = measure_distance();
 194   if (d < 10) menu_ultrasonic_request();
 195 }
 196 
 197 const uint8_t klavesy_efektov[] PROGMEM = {'v', 'b', 'n', 'j', 'h', 'g', 'f', 's', 'l', 'o', 'i', 'u', 'y', 'r', 'e', 'w'};
 198 #define POCET_KLAVES_EFEKTOV 16
 199 
 200 uint8_t vydaj_zvukovy_efekt(char z)
 201 {
 202   for (int i = 0; i < POCET_KLAVES_EFEKTOV; i++)
 203     if (z == pgm_read_byte_near(klavesy_efektov + i)) 
 204     {
 205       zvukovy_efekt(i + 1);
 206       return 1;
 207     }
 208   return 0;
 209 }
 210 
 211 uint8_t ma_zahrat_melodiu(char z)
 212 {
 213   if (z == 9) zahraj_melodiu(1);
 214   else if (z == '*') zahraj_melodiu(2);
 215   else if (z == '/') zahraj_melodiu(3);
 216   else if (z == '+') zahraj_melodiu(4);
 217 }
 218 
 219 void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del)
 220 {
 221   if (*note == 251)
 222   {
 223     *freq = ((*(note + 1)) << 8) + (*(note + 2));
 224     uint8_t d1 = *(note + 3);
 225     uint8_t d2 = *(note + 4);
 226     *del = (music_speed * 16 * (long)d1) / d2;
 227   }
 228   else if (*note == 254)
 229   {
 230     *freq = FIS3;
 231     *del = music_speed;
 232   }
 233   else if (*note == 255)
 234   {
 235     *freq = G3;
 236     *del = music_speed;
 237   }
 238   else
 239   {
 240     uint8_t len = *note / 50; 
 241     *del = music_speed;
 242     while (len--) *del *= 2;
 243     uint8_t n = *note % 50;
 244     if (n != 49)
 245     {
 246       uint8_t octave = (n + 5) / 12;
 247       n = (n + 5 ) % 12;
 248       float ffreq = octave_4[n];
 249       octave = 4 - octave;
 250       while (octave > 0)
 251       {
 252         ffreq /= 2.0;
 253         octave--;  
 254       }
 255     *freq = (uint16_t)ffreq;
 256     }
 257   else *freq = 0;
 258   }
 259 }
 260 
 261 void otto_tone(uint8_t *note)
 262 {
 263   uint16_t freq, d;
 264   otto_translate_tone(note, &freq, &d);
 265   if (freq) tone(SIRENA, freq); else noTone(SIRENA);
 266   delay(d);
 267   noTone(SIRENA);
 268 }
 269 
 270 void zahraj_melodiu(uint8_t cislo) 
 271 {
 272   uint8_t nota[5];
 273   music_speed = 800 / 16;
 274   for (int i = 0; i < dlzka_melodia[cislo]; i++)
 275   {
 276       if (serial_available() || Serial.available()) break;
 277       switch(cislo){
 278         case 1: nota[0] = pgm_read_byte_near(melodia1 + i); break;
 279         case 2: nota[0] = pgm_read_byte_near(melodia2 + i); break;
 280         case 3: nota[0] = pgm_read_byte_near(melodia3 + i); break;
 281         case 4: nota[0] = pgm_read_byte_near(melodia4 + i); break;
 282       }
 283       if (nota[0] == 252)
 284       {
 285         switch(cislo){
 286           case 1: music_speed = pgm_read_byte_near(melodia1 + i + 1); break;
 287           case 2: music_speed = pgm_read_byte_near(melodia2 + i + 1); break;
 288           case 3: music_speed = pgm_read_byte_near(melodia3 + i + 1); break;
 289           case 4: music_speed = pgm_read_byte_near(melodia4 + i + 1); break;
 290         }       
 291         i++;
 292         continue;
 293       }
 294       if (nota[0] == 251)
 295       {
 296         for (int k = 1; k < 5; k++)
 297           switch(cislo){
 298             case 1: nota[k] = pgm_read_byte_near(melodia1 + i + k); break;
 299             case 2: nota[k] = pgm_read_byte_near(melodia2 + i + k); break;
 300             case 3: nota[k] = pgm_read_byte_near(melodia3 + i + k); break;
 301             case 4: nota[k] = pgm_read_byte_near(melodia4 + i + k); break;
 302           }                
 303         i += 4;
 304       }
 305       otto_tone(nota);      
 306     }
 307 }
 308 
 309 void efekt1()
 310 {
 311   uint16_t fre = 100;
 312   for (int i = 0; i < 200; i++)
 313   {
 314     tone(SIRENA, fre);
 315     fre += 10;
 316     delay(2); 
 317   }
 318   noTone(SIRENA);
 319 }
 320 
 321 void efekt2()
 322 {
 323   uint16_t fre = 2000;
 324   for (int i = 0; i < 200; i++)
 325   {
 326     tone(SIRENA, fre);
 327     fre -= 10;
 328     delay(2); 
 329   }
 330   noTone(SIRENA);
 331 }
 332 
 333 void efekt3()
 334 {
 335   uint16_t fre;
 336   for (int i = 0; i < 200; i++)
 337   {
 338     fre = random(3000) + 20;
 339     tone(SIRENA, fre);    
 340     delay(2); 
 341   }
 342   noTone(SIRENA);
 343 }
 344 
 345 void efekt4()
 346 {
 347   uint16_t fre = 100;
 348   for (int i = 0; i < 200; i++)
 349   {
 350     (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
 351     fre += 10;
 352     delay(2); 
 353   }
 354   noTone(SIRENA);
 355 }
 356 
 357 void efekt5()
 358 {
 359   uint16_t fre = 100;
 360   for (int i = 0; i < 100; i++)
 361   {
 362     (i&1) ? tone(SIRENA, fre) : tone(SIRENA, 2100 - fre);
 363     fre += 20;
 364     delay(4); 
 365   }
 366   noTone(SIRENA);
 367 }
 368 
 369 void efekt6()
 370 {
 371   uint16_t d = 12;
 372   for (int i = 0; i < 20; i++)
 373   {
 374     tone(SIRENA, 1760, 10);    
 375     delay(d);
 376     d += 3;
 377   }
 378 }
 379 
 380 void efekt7()
 381 {
 382   for (int i = 0; i < 40; i++)
 383   {
 384     (i&1) ? noTone(SIRENA): tone(SIRENA, 1760);
 385     delay(10); 
 386   }
 387   noTone(SIRENA);
 388 }
 389 
 390 void efekt8()
 391 {
 392   uint16_t d = 72;
 393   for (int i = 0; i < 20; i++)
 394   {
 395     tone(SIRENA, 1760, 10);    
 396     delay(d);
 397     d -= 3;
 398   }
 399 }
 400 
 401 void efekt9()
 402 {
 403   float fre = 3500;
 404   while (fre > 15)
 405   {
 406     tone(SIRENA, (uint16_t)fre);
 407     fre *= 0.97;    
 408     delay(2); 
 409   }
 410   noTone(SIRENA);
 411 }
 412 
 413 void efekt10()
 414 {
 415   float fre = 55;
 416   while (fre < 3000)
 417   {
 418     tone(SIRENA, (uint16_t)fre);
 419     fre *= 1.03;    
 420     delay(2); 
 421   }
 422   noTone(SIRENA);
 423 }
 424 
 425 void efekt11()
 426 {
 427   uint16_t fre = 3500;
 428   uint16_t d = 42;
 429   for (int i = 0; i < 20; i++)
 430   {
 431     tone(SIRENA, fre);
 432     fre -= 165; 
 433     delay(d);
 434     d -= 2;
 435   }
 436   noTone(SIRENA);
 437 }
 438 
 439 void efekt12()
 440 {
 441   float fre = 110;
 442   uint8_t d = 42;
 443   for (int i = 0; i < 20; i++)
 444   {
 445     tone(SIRENA, fre);
 446     fre += 165; 
 447     delay(d);
 448     d -= 2;
 449   }
 450   noTone(SIRENA);
 451 }
 452 
 453 void efekt13()
 454 {
 455   uint16_t fre = 3400;
 456   uint8_t d = 200;
 457   uint8_t delta = 1;
 458   for (int i = 0; i < 20; i++)
 459   {
 460     tone(SIRENA, fre);
 461     fre -= 165; 
 462     delay(d);
 463     d -= delta;
 464     delta++;
 465   }
 466   tone(SIRENA, 110);
 467   delay(1000);
 468   noTone(SIRENA);
 469 }
 470 
 471 void efekt14()
 472 {
 473   uint16_t fre = 880;  
 474   int16_t d = 20;
 475   for (int i = 0; i < 20; i++)
 476   {
 477     tone(SIRENA, fre);
 478     delay(d);
 479     fre += random(50) - 25;
 480     d += random(10) - 5;
 481     if (d < 0) d = 1;
 482   }
 483   noTone(SIRENA);
 484 }
 485 
 486 void efekt15()
 487 {
 488   uint16_t fre = 440;  
 489   int16_t d = 20;
 490   for (int i = 0; i < 20; i++)
 491   {
 492     tone(SIRENA, fre);
 493     delay(d);
 494     fre += random(25) - 12;
 495     d += random(10) - 5;
 496     if (d < 0) d = 1;
 497   }
 498   noTone(SIRENA);
 499 }
 500 
 501 void efekt16()
 502 {
 503   uint16_t fre = 1760;  
 504   int16_t d = 20;
 505   for (int i = 0; i < 20; i++)
 506   {
 507     tone(SIRENA, fre);
 508     delay(d);
 509     fre += random(100) - 50;
 510     d += random(10) - 5;
 511     if (d < 0) d = 1;
 512   }
 513   noTone(SIRENA);
 514 }
 515 
 516 void zvukovy_efekt(uint8_t cislo)
 517 {
 518   switch (cislo) {
 519     case 1: efekt1(); break;
 520     case 2: efekt2(); break;
 521     case 3: efekt3(); break;
 522     case 4: efekt4(); break;
 523     case 5: efekt5(); break; 
 524     case 6: efekt6(); break; 
 525     case 7: efekt7(); break; 
 526     case 8: efekt8(); break; 
 527     case 9: efekt9(); break; 
 528     case 10: efekt10(); break; 
 529     case 11: efekt11(); break; 
 530     case 12: efekt12(); break;
 531     case 13: efekt13(); break;  
 532     case 14: efekt14(); break;  
 533     case 15: efekt15(); break;  
 534     case 16: efekt16(); break;  
 535   }
 536 }
 537 
 538 void test_ultrazvuk()
 539 {
 540   int i = 0;
 541   while ((serial_available() == 0) && (Serial.available() == 0))
 542   {
 543     serial_println_num(measure_distance());
 544     delay(100);
 545   }
 546   serial_read();
 547 }
 548 
 549 void menu_ultrasonic_request()
 550 {
 551   uint32_t tm = millis();
 552   int d = measure_distance();
 553   int count = 0;
 554   int count2 = 0;
 555   while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10)) 
 556   {
 557     delay(10);
 558     d = measure_distance();
 559     if (d == 10000) count++;
 560     else count = 0;
 561     if (d >= 15) count2++;
 562     else count2 = 0;
 563     if (serial_available() || Serial.available()) return;
 564   }
 565   if (millis() - tm >= 1500)
 566     ultrasonic_menu();
 567 }
 568 
 569 void ultrasonic_menu()
 570 {
 571   int selection = 0;
 572   tone(SIRENA, 880, 200);
 573   
 574   do {  
 575     int count = 0;
 576     do { 
 577       int32_t d = measure_distance();
 578       if (d == 10000) continue;
 579       if (d >= 20) count++;
 580       else count = 0;
 581       delay(10);
 582     } while (!serial_available() && !Serial.available() && (count < 20));
 583     
 584     tone(SIRENA, 440, 200);
 585     uint32_t tm = millis();
 586     while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
 587     if (millis() - tm >= 1500) 
 588     {
 589       tone(SIRENA, 2000, 50);
 590       menu_command(selection);
 591       return;
 592     }       
 593     selection++;
 594     for (int i = 0; i < selection; i++)
 595     {
 596       tone(SIRENA, 1261, 50);
 597       delay(250);
 598     }
 599   } while (!serial_available() && !Serial.available());
 600   while (serial_available()) serial_read();
 601   while (Serial.available()) Serial.read();
 602 }
 603 
 604 void menu_command(int cmd)
 605 {
 606   if (cmd == 1) vpred();
 607   if (cmd == 2) zahraj_melodiu(1);
 608   if (cmd == 3) melodia_jedna_druhej();
 609   if (cmd == 4) ahoj();
 610   if (cmd == 5) zahraj_melodiu(2);
 611   if (cmd == 6) zahraj_melodiu(3);
 612   if (cmd == 7) zahraj_melodiu(4);
 613   serial_println_num(cmd);
 614 }
 615 
 616 void melodia_jedna_druhej()
 617 {
 618   for (int i = 0; i < 2; i++)
 619   {
 620     tone(SIRENA, 262, 200);
 621     delay(200);
 622     tone(SIRENA, 330, 200);
 623     delay(200);
 624   
 625     tone(SIRENA, 262, 200);
 626     delay(200);
 627     tone(SIRENA, 330, 200);
 628     delay(200);
 629   
 630     tone(SIRENA, 392, 400);
 631     delay(400);
 632   
 633     tone(SIRENA, 392, 400);
 634     delay(400);
 635   }
 636   noTone(8);
 637 }
 638 
 639 // chodza pre vacsieho dreveneho robota
 640 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
 641 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
 642 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
 643 uint8_t chor_len = 16;
 644 
 645 void vpred()
 646 {
 647   while (measure_distance() > 30)    
 648     zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
 649   pipni();
 650   reset();
 651 }
 652 
 653 void precitaj_kalibraciu_z_EEPROM()
 654 {
 655   uint8_t value = EEPROM.read(1);
 656   if (value != '~') return;
 657   for (int i = 2; i < 8; i++)
 658     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
 659   for (int i = 0; i < 6; i++)
 660     dolny_limit[i] = EEPROM.read(i + 9);
 661   for (int i = 0; i < 6; i++)
 662     horny_limit[i] = EEPROM.read(i + 15);    
 663 }
 664 
 665 char read_char()
 666 {
 667   while (!serial_available() && !Serial.available());
 668   if (serial_available()) return serial_read();
 669   else return Serial.read();
 670 }
 671 
 672 void zapis_kalibraciu_do_EEPROM()
 673 {
 674   serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
 675   char odpoved = read_char();
 676   serial_println_char(odpoved);
 677   if (odpoved == 'Y') 
 678   {
 679     EEPROM.write(1, '~');
 680     for (int i = 2; i < 8; i++)
 681       EEPROM.write(i, kalib[i - 2]);
 682     for (int i = 0; i < 6; i++)
 683       EEPROM.write(9 + i, dolny_limit[i]);
 684     for (int i = 0; i < 6; i++)
 685       EEPROM.write(15 + i, horny_limit[i]);
 686     serial_println("ok");
 687   }
 688 }
 689 
 690 void pipni()
 691 {
 692   tone(SIRENA, 1568, 50);
 693   delay(100);
 694   tone(SIRENA, 1357, 50);
 695 }
 696 
 697 void ruky()
 698 {
 699   int odloz_krok = krok;
 700   delay(500);
 701   krok = 90;
 702   pohyb(SERVO_LAVA_RUKA);    
 703   pohyb(SERVO_PRAVA_RUKA);
 704   delay(1000);
 705   krok = 180;
 706   pohyb(-SERVO_LAVA_RUKA);    
 707   pohyb(-SERVO_PRAVA_RUKA);
 708   delay(1000);
 709   krok = odloz_krok;
 710   pipni();
 711 }
 712 
 713 void ruky2()
 714 {
 715   int odloz_krok = krok;
 716   delay(500);
 717   krok = 180;
 718   pohyb(SERVO_LAVA_RUKA);    
 719   pohyb(SERVO_PRAVA_RUKA);
 720   delay(1000);
 721   krok = 90;
 722   pohyb(-SERVO_LAVA_RUKA);    
 723   pohyb(-SERVO_PRAVA_RUKA);
 724   delay(1000);
 725   krok = odloz_krok;
 726   pipni();
 727 }
 728 
 729 void ahoj()
 730 {
 731   tone(SIRENA, 1568, 50);
 732   delay(70);
 733   tone(SIRENA, 1175, 30);
 734   delay(50);
 735   tone(SIRENA, 880, 30);
 736   delay(50);
 737   tone(SIRENA, 1047, 50);
 738   delay(70);
 739   tone(SIRENA, 1245, 30);
 740   delay(150);
 741   tone(SIRENA, 1568, 50);
 742   delay(100);
 743   if (random(10) > 4) tone(SIRENA, 1357, 50);
 744   else tone(SIRENA, 1047, 50);
 745 }
 746 
 747 void nastav_koncatinu(int8_t servo, uint8_t poloha)
 748 {
 749   int8_t delta;
 750   int8_t srv = (servo > 0)?servo:-servo;
 751   srv--;
 752   poloha += kalib[srv] - 90;
 753   if (poloha > 180) poloha = 180;
 754   if (poloha < 0) poloha = 0;
 755 
 756   if (stav[srv] < poloha) delta = 1;
 757   else delta = -1;
 758   while (stav[srv] != poloha)
 759   {
 760     stav[srv] +=delta;
 761     s[srv].write(stav[srv]);
 762     delay(spomalenie);
 763   }
 764 }
 765 
 766 void zobraz_stav()
 767 {
 768   for (int i = 0; i < 6; i++)
 769   {
 770     serial_print("S"); serial_print_num(i + 1); serial_print(": "); serial_println_num(stav[i] - kalib[i] + 90);
 771   }
 772   serial_println("---");
 773   pipni();
 774 }
 775 
 776 void pohyb(int8_t servo)
 777 {
 778   int8_t srv = (servo > 0)?servo:-servo;
 779   srv--;
 780   if (servo_invertovane[srv]) servo = -servo;
 781   if (servo > 0)
 782   {
 783     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
 784     else stav[srv] = horny_limit[srv];
 785     s[srv].write(stav[srv]);
 786   }
 787   else if (servo < 0)
 788   {
 789     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok; 
 790     else stav[srv] = dolny_limit[srv];
 791     s[srv].write(stav[srv]);      
 792   }
 793 }
 794 
 795 uint8_t pohyb_znakom(char z)
 796 {
 797   for (int i = 0; i < 12; i++)
 798   {
 799     if (z == znaky_zmien[i])
 800     {
 801       int8_t servo = zmeny[i];
 802       pohyb(servo);
 803     }
 804   }
 805 }
 806 
 807 void kombinacia1()
 808 {
 809   pohyb(SERVO_LAVA_NOHA);
 810   pohyb(-SERVO_PRAVA_PATA);
 811 }
 812 
 813 void kombinacia2()
 814 {
 815   pohyb(SERVO_PRAVA_NOHA);
 816   pohyb(-SERVO_LAVA_PATA);
 817 }
 818 
 819 void kombinacia3()
 820 {
 821    pohyb(SERVO_LAVA_RUKA); 
 822    pohyb(SERVO_PRAVA_RUKA); 
 823 }
 824 
 825 void kombinacia4()
 826 {
 827    pohyb(-SERVO_LAVA_RUKA); 
 828    pohyb(-SERVO_PRAVA_RUKA); 
 829 }
 830 
 831 int pohyb_kombinacia(char z)
 832 {
 833   if (z == '3') kombinacia1();
 834   else if (z == '4') kombinacia2();
 835   else if (z == '5') kombinacia3();
 836   else if (z == '6') kombinacia4();
 837   else return 0;
 838   return 1;
 839 }
 840 
 841 int nacitajCislo()
 842 {
 843   int num = 0;
 844   int z;
 845   do {
 846     z = read_char();
 847     if (z == '#') while (z != 13) z = read_char();
 848   } while ((z < '0') || (z > '9'));
 849   while ((z >= '0') && (z <= '9'))
 850   {
 851     num *= 10;
 852     num += (z - '0');
 853     do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
 854   }
 855   return num;
 856 }
 857 
 858 void nacitaj_choreografiu()
 859 {
 860   ch_len = 0;
 861   int tm;
 862   do { 
 863     tm = nacitajCislo();
 864     ch_time[ch_len] = tm;
 865     ch_servo[ch_len] = nacitajCislo();
 866     ch_val[ch_len] = nacitajCislo();
 867     ch_len++;
 868   if (ch_len == CHOREO_LEN) break;
 869   } while (tm > 0);
 870   pipni();  
 871 }
 872 
 873 void vypis_choreografiu()
 874 {
 875   for (int i = 0; i < ch_len; i++)
 876   {
 877     serial_print_num(ch_time[i]);
 878     serial_print(" ");
 879     serial_print_num(ch_servo[i]);
 880     serial_print(" ");
 881     serial_println_num(ch_val[i]);      
 882   }
 883   serial_println("---");
 884   pipni();
 885 }
 886 
 887 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
 888 {
 889   for (int i = 0; i < ch_len - 1; i++)
 890   {
 891     delay(ch_time[i]);
 892     if (ch_servo[i] < 10) nastav_koncatinu(ch_servo[i], ch_val[i]);
 893     else specialny_prikaz(ch_servo[i], ch_val[i]);
 894   }
 895   pipni();
 896   while (serial_available()) serial_read();
 897   while (Serial.available()) Serial.read();
 898 }
 899 
 900 void specialny_prikaz(uint8_t prikaz, uint8_t argument)
 901 {
 902   if (prikaz == 10) spomalenie = argument;
 903   else if (prikaz == 11) zacni_hrat_melodiu(argument);
 904   else if (prikaz == 12) zvukovy_efekt(argument);
 905 }
 906 
 907 void zacni_hrat_melodiu(uint8_t cislo)
 908 {
 909   
 910 }
 911 
 912 void reset()
 913 {
 914   for (int i = 0; i < 6; i++) 
 915   {
 916     stav[i] = kalib[i];
 917     s[i].write(kalib[i]);
 918     delay(300);
 919   }
 920   pipni();
 921 }
 922 
 923 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
 924 {
 925     serial_print(" (+/-/ENTER): ");
 926     serial_println_num(hodnota);
 927     s[servo].write(hodnota);
 928     char z;
 929     do {
 930       z = read_char();
 931       if ((z == '+') && (hodnota < 180)) hodnota++;
 932       else if ((z == '-') && (hodnota > 0)) hodnota--; 
 933       if ((z == '+') || (z == '-'))
 934       {
 935         serial_print_num(hodnota); serial_print_char('\r'); 
 936         s[servo].write(hodnota);
 937       }
 938     } while (z != 13);
 939     return hodnota;
 940 }
 941 
 942 void kalibruj()
 943 {
 944   for (int i = 0; i < 6; i++)
 945   {
 946     serial_print_num(i);
 947     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
 948     serial_print_num(i);
 949     serial_print(": ");
 950     serial_println_num(kalib[i]);
 951   }
 952   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
 953   serial_println("ok");
 954   pipni();
 955 }
 956 
 957 void nastav_limity()
 958 {
 959   for (int i = 0; i < 6; i++)
 960   {
 961     serial_print_num(i);
 962     serial_print("dolny");
 963     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
 964     serial_print_num(i);
 965     serial_print(" dolny: ");
 966     serial_println_num(dolny_limit[i]);
 967     s[i].write(kalib[i]);
 968 
 969     serial_print_num(i);
 970     serial_print("horny");
 971     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
 972     serial_print_num(i);
 973     serial_print(" horny: ");
 974     serial_println_num(horny_limit[i]);
 975     s[i].write(kalib[i]);
 976   }
 977   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
 978   serial_println("ok");
 979   pipni();
 980 }
 981 
 982 void vypis_kalibraciu()
 983 {
 984   serial_print("stredy: ");
 985   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
 986   serial_println();
 987   serial_print("dolny limit: ");
 988   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
 989   serial_println();
 990   serial_print("horny limit: ");
 991   for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
 992   serial_println();
 993 }
 994 
 995 void nacitaj_kalibraciu()
 996 {
 997   int tm;
 998   for (int i = 0; i < 6; i++) 
 999     kalib[i] = nacitajCislo();
1000   vypis_kalibraciu();
1001   serial_println("ok");
1002   pipni();
1003 }
1004 
1005 void zvys_krok()
1006 {
1007   if (krok < 180) krok++;
1008   serial_print("krok: ");
1009   serial_println_num(krok);
1010 }
1011 
1012 void zniz_krok()
1013 {
1014   if (krok > 0) krok--;
1015   serial_print("krok: ");
1016   serial_println_num(krok);
1017 }
1018 
1019 void zniz_spomalenie()
1020 {
1021   if (spomalenie > 0) spomalenie--;
1022   serial_print("spomalenie: "); 
1023   serial_println_num(spomalenie);
1024 }
1025 
1026 void zvys_spomalenie()
1027 {
1028   if (spomalenie < 100) spomalenie++;
1029   serial_print("spomalenie: ");
1030   serial_println_num(spomalenie);
1031 }
1032 
1033 // nasleduje softverova implementacia serioveho portu
1034 #define SERIAL_STATE_IDLE      0
1035 #define SERIAL_STATE_RECEIVING 1
1036 #define SERIAL_BUFFER_LENGTH   20
1037 
1038 static volatile uint8_t serial_state;
1039 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
1040 static volatile uint8_t serial_buf_wp, serial_buf_rp;
1041 
1042 static volatile uint8_t receiving_byte;
1043 
1044 static volatile uint32_t time_startbit_noticed;
1045 static volatile uint8_t next_bit_order;
1046 static volatile uint8_t waiting_stop_bit;
1047 static uint16_t one_byte_duration;
1048 static uint16_t one_bit_duration;
1049 static uint16_t one_bit_write_duration;
1050 static uint16_t half_of_one_bit_duration;
1051 
1052 void init_serial(uint32_t baud_rate)
1053 {
1054   pinMode(2, INPUT);
1055   pinMode(4, OUTPUT);
1056   
1057   serial_state = SERIAL_STATE_IDLE;
1058   
1059   one_byte_duration = 9500000 / baud_rate;
1060   one_bit_duration = 1000000 / baud_rate;
1061   one_bit_write_duration = one_bit_duration - 1;
1062   half_of_one_bit_duration = 500000 / baud_rate;
1063   
1064   PCMSK2 |= 4; //PCINT18;
1065   PCIFR &= ~4; //PCIF2;
1066   PCICR |= 4; // PCIE2;
1067 }
1068 
1069 ISR(PCINT2_vect)
1070 {
1071   uint32_t tm = micros();
1072   if (serial_state == SERIAL_STATE_IDLE)
1073   {    
1074     time_startbit_noticed = tm;
1075     serial_state = SERIAL_STATE_RECEIVING;
1076     receiving_byte = 0xFF;
1077     next_bit_order = 0;
1078   }
1079   else if (tm - time_startbit_noticed > one_byte_duration)
1080   {
1081       serial_buffer[serial_buf_wp] = receiving_byte;
1082       serial_buf_wp++;
1083       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1084       time_startbit_noticed = tm;
1085       receiving_byte = 0xFF;
1086       next_bit_order = 0;
1087   }
1088   else if (PIND & 4) 
1089   {
1090      int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1091      while (next_bit_order < new_next_bit_order)
1092      {  
1093         receiving_byte &= ~(1 << next_bit_order);
1094         next_bit_order++;
1095      }
1096      if (next_bit_order == 8)
1097      { 
1098         serial_buffer[serial_buf_wp] = receiving_byte;
1099         serial_buf_wp++;
1100         if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1101         serial_state = SERIAL_STATE_IDLE;
1102      }        
1103   } else 
1104       next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
1105 }
1106 
1107 uint8_t serial_available()
1108 {
1109   cli();
1110   if (serial_buf_rp != serial_buf_wp) 
1111   {
1112     sei();
1113     return 1;
1114   }
1115   if (serial_state == SERIAL_STATE_RECEIVING)
1116   {
1117     uint32_t tm = micros();
1118     if (tm - time_startbit_noticed > one_byte_duration)
1119     {      
1120       serial_state = SERIAL_STATE_IDLE;
1121       serial_buffer[serial_buf_wp] = receiving_byte;
1122       serial_buf_wp++;
1123       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
1124       sei();
1125       return 1;
1126     }
1127   }
1128   sei();
1129   return 0;
1130 }
1131 
1132 int16_t serial_read()
1133 {
1134   cli();
1135   if (serial_buf_rp != serial_buf_wp)
1136   {
1137     uint8_t ch = serial_buffer[serial_buf_rp];
1138     serial_buf_rp++;
1139     if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
1140     sei();
1141     return ch;
1142   }
1143 
1144   if (serial_state == SERIAL_STATE_RECEIVING)
1145   {
1146     uint32_t tm = micros();
1147     if (tm - time_startbit_noticed > one_byte_duration)
1148     {
1149       uint8_t ch = receiving_byte;
1150       serial_state = SERIAL_STATE_IDLE;  
1151       sei();
1152       return ch;
1153     }
1154   }
1155   sei();
1156   return -1;
1157 }
1158 
1159 void serial_write(uint8_t ch)
1160 {
1161 #ifdef ECHO_BT_TO_USB
1162   Serial.print((char)ch);
1163 #endif
1164   PORTD &= ~16;
1165   delayMicroseconds(one_bit_write_duration);
1166   for (uint8_t i = 0; i < 8; i++)
1167   {
1168     if (ch & 1) PORTD |= 16;
1169     else PORTD &= ~16;
1170     ch >>= 1;
1171     delayMicroseconds(one_bit_write_duration);
1172   }
1173   PORTD |= 16;
1174   delayMicroseconds(one_bit_write_duration);
1175   delayMicroseconds(one_bit_write_duration);
1176   delayMicroseconds(one_bit_write_duration);
1177   delayMicroseconds(one_bit_write_duration);
1178   delayMicroseconds(one_bit_write_duration);
1179 }
1180 
1181 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
1182 {
1183   uint16_t len;
1184   uint16_t ch;
1185   do {
1186     ch = serial_read();
1187     if (ch == 13) continue;
1188   } while (ch == -1);
1189 
1190   do {
1191     if ((ch != 13) && (ch != 10) && (ch != -1)) 
1192     {
1193       *(ln++) = ch;
1194       max_length--;
1195       len++;
1196     }    
1197     ch = serial_read();
1198   } while ((ch != 13) && max_length);
1199   *ln = 0;
1200   return len;
1201 }
1202 
1203 void serial_print_num(int32_t number)
1204 {
1205   if (number < 0) 
1206   {
1207     serial_write('-');
1208     number = -number;
1209   }
1210   int32_t rad = 1;
1211   while (number / rad) rad *= 10;
1212   if (number > 0) rad /= 10;
1213   while (rad)
1214   {
1215     serial_write((char)('0' + (number / rad)));
1216     number -= (number / rad) * rad;
1217     rad /= 10;
1218   }  
1219 }
1220 
1221 void serial_print_char(char ch)
1222 {
1223   serial_write(ch);  
1224 }
1225 
1226 void serial_print(const uint8_t *str)
1227 {
1228   while (*str) serial_write(*(str++));
1229 }
1230 
1231 void serial_println(const uint8_t *str)
1232 {
1233   serial_print(str);
1234   serial_write(13);
1235   serial_write(10);
1236 }
1237 
1238 void serial_println_num(int32_t number)
1239 {
1240   serial_print_num(number);
1241   serial_println();
1242 }
1243 
1244 void serial_println_char(char ch)
1245 {
1246   serial_write(ch);
1247   serial_println();
1248 }
1249 
1250 void serial_println()
1251 {
1252   serial_write(13);
1253   serial_write(10);
1254 }
1255 
1256 // nasleduje citanie z utltazvukoveho senzora
1257 
1258 static volatile uint32_t pulse_start;
1259 static volatile uint8_t new_distance;
1260 
1261 void init_ultrasonic()
1262 {
1263   pinMode(US_ECHO, INPUT);
1264   pinMode(US_TRIG, OUTPUT);
1265   
1266   PCMSK0 |= 1; //PCINT0;
1267   PCIFR &= ~1; //PCIF0;
1268   PCICR |= 1; // PCIE0;  
1269 }
1270 
1271 ISR(PCINT0_vect)
1272 {
1273   if (PINB & 1) pulse_start = micros();
1274   else 
1275   {
1276     distance = (int16_t)((micros() - pulse_start) / 58);
1277     new_distance = 1;
1278   }
1279 }
1280 
1281 void start_distance_measurement()
1282 {
1283   distance = 10000;
1284   new_distance = 0;
1285   digitalWrite(US_TRIG, HIGH);
1286   delayMicroseconds(10);
1287   digitalWrite(US_TRIG, LOW);
1288 }
1289 
1290 void wait_for_distance_measurement_to_complete()
1291 {
1292   uint8_t counter = 0;
1293   while ((counter < 20) && !new_distance) 
1294   {
1295     delay(1);
1296     counter++;
1297   }
1298   if (counter == 20)
1299   {
1300     pinMode(US_ECHO, OUTPUT);
1301     digitalWrite(US_ECHO, HIGH);
1302     delayMicroseconds(10);
1303     digitalWrite(US_ECHO, LOW);
1304     pinMode(US_ECHO, INPUT); 
1305     delayMicroseconds(5);
1306     distance = 10000;
1307   }
1308 }
1309 
1310 int16_t measure_distance()
1311 {
1312   start_distance_measurement();
1313   wait_for_distance_measurement_to_complete();
1314   return distance;
1315 }