Otto - riadiaci program v.3

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