Otto - riadiaci program v.3

From DT^2
Revision as of 06:05, 10 August 2018 by Admin (talk | contribs)
(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

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

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

Pripojenie na vývody Arduina:

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