Otto - riadiaci program v.3

From DT^2
Revision as of 05:45, 9 August 2018 by Admin (talk | contribs)
Jump to: navigation, search
  1 #include <Servo.h>
  2 #include <EEPROM.h>
  3 
  4 #define ECHO_BT_TO_USB 1
  5 
  6 #define US_TRIG  7
  7 #define US_ECHO  8
  8 
  9 #define BT_RX   2 
 10 #define BT_TX   4
 11 
 12 #define SIRENA 12
 13 
 14 //maximalna dlzka choreografie
 15 #define CHOREO_LEN 200
 16 
 17 // cisla pinov, kde su zapojene servo motory
 18 #define PIN_SERVO_LAVA_RUKA   10 
 19 #define PIN_SERVO_PRAVA_RUKA  11
 20 #define PIN_SERVO_LAVA_NOHA   9
 21 #define PIN_SERVO_PRAVA_NOHA  6
 22 #define PIN_SERVO_LAVA_PATA   5
 23 #define PIN_SERVO_PRAVA_PATA  3
 24 
 25 #define S1 3
 26 #define S2 5
 27 #define S3 6
 28 #define S4 9
 29 #define S5 10
 30 #define S6 11
 31 #define SIRENA 12
 32 
 33 //maximalna dlzka choreografie
 34 #define CHOREO_LEN 200
 35 
 36 // tu su serva cislovane 1-6
 37 #define SERVO_LAVA_RUKA   5 
 38 #define SERVO_PRAVA_RUKA  6
 39 #define SERVO_LAVA_NOHA   4
 40 #define SERVO_PRAVA_NOHA  3
 41 #define SERVO_LAVA_PATA   2
 42 #define SERVO_PRAVA_PATA  1
 43 
 44 // ak su niektore serva naopak, je tu jednotka
 45 uint8_t servo_invertovane[6] = {0, 0, 1, 1, 0, 1};
 46 
 47 // znaky, ktorymi sa ovladaju jednotlive stupne volnosti
 48 char znaky_zmien[] = {'a', 'q', ';', 'p', 'z', 'x', ',', '.', 'd', 'c', 'k', 'm' };
 49 // co robia jednotlive znaky (znamienko urcuje smer)
 50 int8_t zmeny[] = {SERVO_LAVA_RUKA, -SERVO_LAVA_RUKA, 
 51                   SERVO_PRAVA_RUKA, -SERVO_PRAVA_RUKA, 
 52                   -SERVO_LAVA_NOHA, SERVO_LAVA_NOHA, 
 53                   -SERVO_PRAVA_NOHA, SERVO_PRAVA_NOHA, 
 54                    SERVO_LAVA_PATA, -SERVO_LAVA_PATA, 
 55                    SERVO_PRAVA_PATA, -SERVO_PRAVA_PATA }; 
 56 
 57 // sem si mozno ulozit svoju kalibraciu
 58 //uint8_t prednastavena_kalibracia[] = { 78, 69, 83, 80, 50, 67 };
 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 uint8_t *melodia;
 78 uint16_t dlzka_melodie;
 79 
 80 #define FIS3 2960
 81 #define G3 3136
 82 
 83 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 };
 84 int16_t music_speed;
 85 
 86 uint16_t dlzka_popcorn = 390;
 87 uint8_t popcorn[] = { 149, 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, 49,
 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, 149,
 90 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, 
 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, 49,
 92 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,
 93 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,
 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, 
 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, 149 ,
 96 40, 49, 99, 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, 
 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, 
 98 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 ,
 99 40, 49, 99, 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
100 };
101 
102 void setup() {
103   Serial.begin(9600);
104   init_serial(9600);
105   init_ultrasonic();
106 
107   melodia = popcorn;
108   dlzka_melodie = dlzka_popcorn;
109 
110   randomSeed(analogRead(1));
111   s[0].attach(PIN_SERVO_PRAVA_PATA);
112   s[1].attach(PIN_SERVO_LAVA_PATA);
113   s[2].attach(PIN_SERVO_PRAVA_NOHA);
114   s[3].attach(PIN_SERVO_LAVA_NOHA);
115   s[4].attach(PIN_SERVO_LAVA_RUKA);
116   s[5].attach(PIN_SERVO_PRAVA_RUKA);
117   precitaj_kalibraciu_z_EEPROM();
118   for (int i = 0; i < 6; i++)
119   {
120     kalib[i] = prednastavena_kalibracia[i];
121     stav[i] = kalib[i];
122     s[i].write(stav[i]);
123   }
124   ch_len = 0;
125   krok = 7;
126   spomalenie = 6;
127   ahoj();
128   ruky2();
129   delay(100);  
130   serial_println("\r\n  Otto DTDT");
131 }
132 
133 void loop() {
134   char z = -1;
135   if (serial_available()) z = serial_read();
136 #ifdef ECHO_BT_TO_USB
137     if (Serial.available()) z = Serial.read();
138 #endif  
139   
140   if (z != -1)
141   { 
142     if (pohyb_znakom(z)) return;   
143     else if (pohyb_kombinacia(z)) return; 
144     else if (z == '@') nacitaj_choreografiu();
145     else if (z == '?') vypis_choreografiu();
146     else if (z == 't') zatancuj_choreografiu(ch_time, ch_servo, ch_val, ch_len);
147     else if (z == '-') ahoj();
148     else if (z == ' ') reset();
149     else if (z == 'H') kalibruj();
150     else if (z == 'J') nastav_limity();
151     else if (z == 'G') vypis_kalibraciu();
152     else if (z == 'L') nacitaj_kalibraciu();
153     else if (z == 'E') zapis_kalibraciu_do_EEPROM();
154     else if (z == 'R') ruky();
155     else if (z == '9') zvys_krok();
156     else if (z == '1') zniz_krok();
157     else if (z == '8') zvys_spomalenie();
158     else if (z == '7') zniz_spomalenie();
159     else if (z == 'U') test_ultrazvuk();
160     else if (z == 'S') zobraz_stav();
161   }
162   int16_t d = measure_distance();
163   if (d < 10) menu_ultrasonic_request();
164 }
165 
166 void otto_translate_tone(uint8_t *note, uint16_t *freq, uint16_t *del)
167 {
168   if (*note == 251)
169   {
170     *freq = ((*(note + 1)) << 8) + (*(note + 2));
171     uint8_t d1 = *(note + 3);
172     uint8_t d2 = *(note + 4);
173     *del = (music_speed * 16 * (long)d1) / d2;
174   }
175   else if (*note == 254)
176   {
177     *freq = FIS3;
178 	*del = music_speed;
179   }
180   else if (*note == 255)
181   {
182     *freq = G3;
183 	*del = music_speed;
184   }
185   else
186   {
187     uint8_t len = *note / 50; 
188     *del = music_speed;
189     while (len--) *del *= 2;
190     uint8_t n = *note % 50;
191     if (n != 49)
192     {
193       uint8_t octave = (n + 5) / 12;
194       n = (n + 5 ) % 12;
195       float ffreq = octave_4[n];
196       octave = 4 - octave;
197       while (octave > 0)
198       {
199         ffreq /= 2.0;
200         octave--;  
201       }
202 	  *freq = (uint16_t)ffreq;
203     }
204 	else *ffreq = 0;
205   }
206 }
207 
208 void otto_tone(uint8_t *note)
209 {
210   uint16_t freq, d;
211   otto_translate_tone(note, &freq, &d);
212   if (freq) tone(SIRENA, freq); else noTone(SIRENA);
213   delay(d);
214   noTone(SIRENA);
215 }
216 
217 void zahraj_melodiu() 
218 {
219   music_speed = 800 / 16;
220   for (int j = 0; j < 4; j++)
221   {
222     for (int i = 0; i < dlzka_melodie; i++)
223     {
224       otto_tone(melodia + i);
225       if (melodia[i] == 251) i += 4;
226     }
227     music_speed -= 150 / 16;
228   }
229   delay(3000);
230 }
231 
232 void test_ultrazvuk()
233 {
234   int i = 0;
235   while ((serial_available() == 0) && (Serial.available() == 0))
236   {
237     serial_println_num(measure_distance());
238     delay(100);
239   }
240   serial_read();
241 }
242 
243 void menu_ultrasonic_request()
244 {
245   uint32_t tm = millis();
246   int d = measure_distance();
247   int count = 0;
248   int count2 = 0;
249   while ((millis() - tm < 1500) && ((d < 15) || (count2 < 5)) && (count < 10)) 
250   {
251     delay(10);
252     d = measure_distance();
253     if (d == 10000) count++;
254     else count = 0;
255     if (d >= 15) count2++;
256     else count2 = 0;
257 	if (serial_available() || Serial.available()) return;
258   }
259   if (millis() - tm >= 1500)
260     ultrasonic_menu();
261 }
262 
263 void ultrasonic_menu()
264 {
265   int selection = 0;
266   tone(SIRENA, 880, 200);
267   
268   do {  
269     int count = 0;
270     do { 
271       int32_t d = measure_distance();
272       if (d == 10000) continue;
273       if (d >= 20) count++;
274       else count = 0;
275       delay(10);
276     } while (!serial_available() && !Serial.available() && (count < 20));
277     
278     tone(SIRENA, 440, 200);
279     uint32_t tm = millis();
280     while ((measure_distance() > 15) && (millis() - tm < 1500) && !serial_available() && !Serial.available()) delay(10);
281     if (millis() - tm >= 1500) 
282     {
283       tone(SIRENA, 2000, 50);
284       menu_command(selection);
285       return;
286     }       
287     selection++;
288     for (int i = 0; i < selection; i++)
289     {
290       tone(SIRENA, 1261, 50);
291       delay(250);
292     }
293   } while (!serial_available() && !Serial.available());
294   while (serial_available()) serial_read();
295   while (Serial.available()) Serial.read();
296 }
297 
298 void menu_command(int cmd)
299 {
300   if (cmd == 1) vpred();
301   if (cmd == 2) zahraj_melodiu();
302   if (cmd == 3) melodia_jedna_druhej();
303   if (cmd == 4) ahoj();
304   serial_println_num(cmd);
305 }
306 
307 void melodia_jedna_druhej()
308 {
309   for (int i = 0; i < 2; i++)
310   {
311     tone(SIRENA, 262, 200);
312     delay(200);
313     tone(SIRENA, 330, 200);
314     delay(200);
315   
316     tone(SIRENA, 262, 200);
317     delay(200);
318     tone(SIRENA, 330, 200);
319     delay(200);
320   
321     tone(SIRENA, 392, 400);
322     delay(400);
323   
324     tone(SIRENA, 392, 400);
325     delay(400);
326   }
327   noTone(8);
328 }
329 
330 // chodza pre vacsieho dreveneho robota
331 uint16_t chor_time[] = {100,1,1,100,1,1,1,100,1,1,1,100,1,100,1,0};
332 uint8_t chor_servo[] = {1,2,6,4,3,6,5,1,2,6,5,3,4,1,2,0};
333 uint8_t chor_val[] = {48,69,180,104,104,90,0,111,146,0,90,62,69,48,69,0};
334 uint8_t chor_len = 16;
335 
336 void vpred()
337 {
338   while (measure_distance() > 30)    
339     zatancuj_choreografiu(chor_time, chor_servo, chor_val, chor_len);
340   pipni();
341   reset();
342 }
343 
344 void precitaj_kalibraciu_z_EEPROM()
345 {
346   uint8_t value = EEPROM.read(1);
347   if (value != '~') return;
348   for (int i = 2; i < 8; i++)
349     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
350   for (int i = 0; i < 6; i++)
351     dolny_limit[i] = EEPROM.read(i + 9);
352   for (int i = 0; i < 6; i++)
353     horny_limit[i] = EEPROM.read(i + 15);    
354 }
355 
356 char read_char()
357 {
358   while (!serial_available() && !Serial.available());
359   if (serial_available()) return serial_read();
360   else return Serial.read();
361 }
362 
363 void zapis_kalibraciu_do_EEPROM()
364 {
365   serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
366   char odpoved = read_char();
367   serial_println_char(odpoved);
368   if (odpoved == 'Y') 
369   {
370     EEPROM.write(1, '~');
371     for (int i = 2; i < 8; i++)
372       EEPROM.write(i, kalib[i - 2]);
373     for (int i = 0; i < 6; i++)
374       EEPROM.write(9 + i, dolny_limit[i]);
375     for (int i = 0; i < 6; i++)
376       EEPROM.write(15 + i, horny_limit[i]);
377     serial_println("ok");
378   }
379 }
380 
381 void pipni()
382 {
383   tone(SIRENA, 1568, 50);
384   delay(100);
385   tone(SIRENA, 1357, 50);
386 }
387 
388 void ruky()
389 {
390   int odloz_krok = krok;
391   delay(500);
392   krok = 90;
393   pohyb(SERVO_LAVA_RUKA);    
394   pohyb(SERVO_PRAVA_RUKA);
395   delay(1000);
396   krok = 180;
397   pohyb(-SERVO_LAVA_RUKA);    
398   pohyb(-SERVO_PRAVA_RUKA);
399   delay(1000);
400   krok = odloz_krok;
401   pipni();
402 }
403 
404 void ruky2()
405 {
406   int odloz_krok = krok;
407   delay(500);
408   krok = 180;
409   pohyb(SERVO_LAVA_RUKA);    
410   pohyb(SERVO_PRAVA_RUKA);
411   delay(1000);
412   krok = 90;
413   pohyb(-SERVO_LAVA_RUKA);    
414   pohyb(-SERVO_PRAVA_RUKA);
415   delay(1000);
416   krok = odloz_krok;
417   pipni();
418 }
419 
420 void ahoj()
421 {
422   tone(SIRENA, 1568, 50);
423   delay(70);
424   tone(SIRENA, 1175, 30);
425   delay(50);
426   tone(SIRENA, 880, 30);
427   delay(50);
428   tone(SIRENA, 1047, 50);
429   delay(70);
430   tone(SIRENA, 1245, 30);
431   delay(150);
432   tone(SIRENA, 1568, 50);
433   delay(100);
434   if (random(10) > 4) tone(SIRENA, 1357, 50);
435   else tone(SIRENA, 1047, 50);
436 }
437 
438 void nastav_koncatinu(int8_t servo, uint8_t poloha)
439 {
440   int8_t delta;
441   int8_t srv = (servo > 0)?servo:-servo;
442   srv--;
443   poloha += kalib[srv] - 90;
444   if (poloha > 180) poloha = 180;
445   if (poloha < 0) poloha = 0;
446 
447   if (stav[srv] < poloha) delta = 1;
448   else delta = -1;
449   while (stav[srv] != poloha)
450   {
451     stav[srv] +=delta;
452     s[srv].write(stav[srv]);
453     delay(spomalenie);
454   }
455 }
456 
457 void zobraz_stav()
458 {
459   for (int i = 0; i < 6; i++)
460   {
461     serial_print("S"); serial_print_num(i + 1); serial_print(": "); serial_println_num(stav[i] - kalib[i] + 90);
462   }
463   serial_println("---");
464   pipni();
465 }
466 
467 void pohyb(int8_t servo)
468 {
469   int8_t srv = (servo > 0)?servo:-servo;
470   srv--;
471   if (servo_invertovane[srv]) servo = -servo;
472   if (servo > 0)
473   {
474     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
475     else stav[srv] = horny_limit[srv];
476     s[srv].write(stav[srv]);
477   }
478   else if (servo < 0)
479   {
480     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok; 
481     else stav[srv] = dolny_limit[srv];
482     s[srv].write(stav[srv]);      
483   }
484 }
485 
486 uint8_t pohyb_znakom(char z)
487 {
488   for (int i = 0; i < 12; i++)
489   {
490     if (z == znaky_zmien[i])
491     {
492       int8_t servo = zmeny[i];
493       pohyb(servo);
494     }
495   }
496 }
497 
498 void kombinacia1()
499 {
500   pohyb(SERVO_LAVA_NOHA);
501   pohyb(-SERVO_PRAVA_PATA);
502 }
503 
504 void kombinacia2()
505 {
506   pohyb(SERVO_PRAVA_NOHA);
507   pohyb(-SERVO_LAVA_PATA);
508 }
509 
510 void kombinacia3()
511 {
512    pohyb(SERVO_LAVA_RUKA); 
513    pohyb(SERVO_PRAVA_RUKA); 
514 }
515 
516 void kombinacia4()
517 {
518    pohyb(-SERVO_LAVA_RUKA); 
519    pohyb(-SERVO_PRAVA_RUKA); 
520 }
521 
522 int pohyb_kombinacia(char z)
523 {
524   if (z == '3') kombinacia1();
525   else if (z == '4') kombinacia2();
526   else if (z == '5') kombinacia3();
527   else if (z == '6') kombinacia4();
528   else return 0;
529   return 1;
530 }
531 
532 int nacitajCislo()
533 {
534   int num = 0;
535   int z;
536   do {
537     z = read_char();
538     if (z == '#') while (z != 13) z = read_char();
539   } while ((z < '0') || (z > '9'));
540   while ((z >= '0') && (z <= '9'))
541   {
542     num *= 10;
543     num += (z - '0');
544     do { z = read_char(); if (z == -1) delayMicroseconds(10); } while (z < 0);
545   }
546   return num;
547 }
548 
549 void nacitaj_choreografiu()
550 {
551   ch_len = 0;
552   int tm;
553   do { 
554     tm = nacitajCislo();
555     ch_time[ch_len] = tm;
556     ch_servo[ch_len] = nacitajCislo();
557     ch_val[ch_len] = nacitajCislo();
558     ch_len++;
559   if (ch_len == CHOREO_LEN) break;
560   } while (tm > 0);
561   pipni();  
562 }
563 
564 void vypis_choreografiu()
565 {
566   for (int i = 0; i < ch_len; i++)
567   {
568     serial_print_num(ch_time[i]);
569     serial_print(" ");
570     serial_print_num(ch_servo[i]);
571     serial_print(" ");
572     serial_println_num(ch_val[i]);      
573   }
574   pipni();
575 }
576 
577 void zatancuj_choreografiu(uint16_t *ch_time, uint8_t *ch_servo, uint8_t *ch_val, int ch_len )
578 {
579   for (int i = 0; i < ch_len - 1; i++)
580   {
581     delay(ch_time[i]);
582     nastav_koncatinu(ch_servo[i], ch_val[i]);
583   }
584 //  if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
585   pipni();
586   while (serial_available()) serial_read();
587   while (Serial.available()) Serial.read();
588 }
589 
590 void reset()
591 {
592   for (int i = 0; i < 6; i++) 
593   {
594     stav[i] = kalib[i];
595     s[i].write(kalib[i]);
596     delay(300);
597   }
598   pipni();
599 }
600 
601 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
602 {
603     serial_print(" (+/-/ENTER): ");
604     serial_println_num(hodnota);
605     s[servo].write(hodnota);
606     char z;
607     do {
608       z = read_char();
609       if ((z == '+') && (hodnota < 180)) hodnota++;
610       else if ((z == '-') && (hodnota > 0)) hodnota--; 
611       if ((z == '+') || (z == '-'))
612       {
613         serial_print_num(hodnota); serial_print_char('\r'); 
614         s[servo].write(hodnota);
615       }
616     } while (z != 13);
617     return hodnota;
618 }
619 
620 void kalibruj()
621 {
622   for (int i = 0; i < 6; i++)
623   {
624     serial_print_num(i);
625     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
626     serial_print_num(i);
627     serial_print(": ");
628     serial_println_num(kalib[i]);
629   }
630   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
631   serial_println("ok");
632   pipni();
633 }
634 
635 void nastav_limity()
636 {
637   for (int i = 0; i < 6; i++)
638   {
639     serial_print_num(i);
640     serial_print("dolny");
641     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
642     serial_print_num(i);
643     serial_print(" dolny: ");
644     serial_println_num(dolny_limit[i]);
645     s[i].write(kalib[i]);
646 
647     serial_print_num(i);
648     serial_print("horny");
649     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
650     serial_print_num(i);
651     serial_print(" horny: ");
652     serial_println_num(horny_limit[i]);
653     s[i].write(kalib[i]);
654   }
655   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
656   serial_println("ok");
657   pipni();
658 }
659 
660 void vypis_kalibraciu()
661 {
662   serial_print("stredy: ");
663   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
664   serial_println();
665   serial_print("dolny limit: ");
666   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
667   serial_println();
668   serial_print("horny limit: ");
669   for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
670   serial_println();
671 }
672 
673 void nacitaj_kalibraciu()
674 {
675   int tm;
676   for (int i = 0; i < 6; i++) 
677     kalib[i] = nacitajCislo();
678   vypis_kalibraciu();
679   serial_println("ok");
680   pipni();
681 }
682 
683 void zvys_krok()
684 {
685   if (krok < 180) krok++;
686   serial_print("krok: ");
687   serial_println_num(krok);
688 }
689 
690 void zniz_krok()
691 {
692   if (krok > 0) krok--;
693   serial_print("krok: ");
694   serial_println_num(krok);
695 }
696 
697 void zniz_spomalenie()
698 {
699   if (spomalenie > 0) spomalenie--;
700   serial_print("spomalenie: "); 
701   serial_println_num(spomalenie);
702 }
703 
704 void zvys_spomalenie()
705 {
706   if (spomalenie < 100) spomalenie++;
707   serial_print("spomalenie: ");
708   serial_println_num(spomalenie);
709 }
710 
711 // nasleduje softverova implementacia serioveho portu
712 #define SERIAL_STATE_IDLE      0
713 #define SERIAL_STATE_RECEIVING 1
714 #define SERIAL_BUFFER_LENGTH   20
715 
716 static volatile uint8_t serial_state;
717 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
718 static volatile uint8_t serial_buf_wp, serial_buf_rp;
719 
720 static volatile uint8_t receiving_byte;
721 
722 static volatile uint32_t time_startbit_noticed;
723 static volatile uint8_t next_bit_order;
724 static volatile uint8_t waiting_stop_bit;
725 static uint16_t one_byte_duration;
726 static uint16_t one_bit_duration;
727 static uint16_t one_bit_write_duration;
728 static uint16_t half_of_one_bit_duration;
729 
730 void init_serial(uint32_t baud_rate)
731 {
732   pinMode(2, INPUT);
733   pinMode(4, OUTPUT);
734   
735   serial_state = SERIAL_STATE_IDLE;
736   
737   one_byte_duration = 9500000 / baud_rate;
738   one_bit_duration = 1000000 / baud_rate;
739   one_bit_write_duration = one_bit_duration - 1;
740   half_of_one_bit_duration = 500000 / baud_rate;
741   
742   PCMSK2 |= 4; //PCINT18;
743   PCIFR &= ~4; //PCIF2;
744   PCICR |= 4; // PCIE2;
745 }
746 
747 ISR(PCINT2_vect)
748 {
749   uint32_t tm = micros();
750   if (serial_state == SERIAL_STATE_IDLE)
751   {    
752     time_startbit_noticed = tm;
753     serial_state = SERIAL_STATE_RECEIVING;
754     receiving_byte = 0xFF;
755     next_bit_order = 0;
756   }
757   else if (tm - time_startbit_noticed > one_byte_duration)
758   {
759       serial_buffer[serial_buf_wp] = receiving_byte;
760       serial_buf_wp++;
761       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
762       time_startbit_noticed = tm;
763       receiving_byte = 0xFF;
764       next_bit_order = 0;
765   }
766   else if (PIND & 4) 
767   {
768      int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
769      while (next_bit_order < new_next_bit_order)
770      {  
771         receiving_byte &= ~(1 << next_bit_order);
772         next_bit_order++;
773      }
774      if (next_bit_order == 8)
775      { 
776         serial_buffer[serial_buf_wp] = receiving_byte;
777         serial_buf_wp++;
778         if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
779         serial_state = SERIAL_STATE_IDLE;
780      }        
781   } else 
782       next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
783 }
784 
785 uint8_t serial_available()
786 {
787   cli();
788   if (serial_buf_rp != serial_buf_wp) 
789   {
790     sei();
791     return 1;
792   }
793   if (serial_state == SERIAL_STATE_RECEIVING)
794   {
795     uint32_t tm = micros();
796     if (tm - time_startbit_noticed > one_byte_duration)
797     {      
798       serial_state = SERIAL_STATE_IDLE;
799       serial_buffer[serial_buf_wp] = receiving_byte;
800       serial_buf_wp++;
801       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
802       sei();
803       return 1;
804     }
805   }
806   sei();
807   return 0;
808 }
809 
810 int16_t serial_read()
811 {
812   cli();
813   if (serial_buf_rp != serial_buf_wp)
814   {
815     uint8_t ch = serial_buffer[serial_buf_rp];
816     serial_buf_rp++;
817     if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
818     sei();
819     return ch;
820   }
821 
822   if (serial_state == SERIAL_STATE_RECEIVING)
823   {
824     uint32_t tm = micros();
825     if (tm - time_startbit_noticed > one_byte_duration)
826     {
827       uint8_t ch = receiving_byte;
828       serial_state = SERIAL_STATE_IDLE;  
829       sei();
830       return ch;
831     }
832   }
833   sei();
834   return -1;
835 }
836 
837 void serial_write(uint8_t ch)
838 {
839 #ifdef ECHO_BT_TO_USB
840   Serial.print((char)ch);
841 #endif
842   PORTD &= ~16;
843   delayMicroseconds(one_bit_write_duration);
844   for (uint8_t i = 0; i < 8; i++)
845   {
846     if (ch & 1) PORTD |= 16;
847     else PORTD &= ~16;
848     ch >>= 1;
849     delayMicroseconds(one_bit_write_duration);
850   }
851   PORTD |= 16;
852   delayMicroseconds(one_bit_write_duration);
853   delayMicroseconds(one_bit_write_duration);
854   delayMicroseconds(one_bit_write_duration);
855   delayMicroseconds(one_bit_write_duration);
856   delayMicroseconds(one_bit_write_duration);
857 }
858 
859 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
860 {
861   uint16_t len;
862   uint16_t ch;
863   do {
864     ch = serial_read();
865     if (ch == 13) continue;
866   } while (ch == -1);
867 
868   do {
869     if ((ch != 13) && (ch != 10) && (ch != -1)) 
870     {
871       *(ln++) = ch;
872       max_length--;
873       len++;
874     }    
875     ch = serial_read();
876   } while ((ch != 13) && max_length);
877   *ln = 0;
878   return len;
879 }
880 
881 void serial_print_num(int32_t number)
882 {
883   if (number < 0) 
884   {
885     serial_write('-');
886     number = -number;
887   }
888   int32_t rad = 1;
889   while (number / rad) rad *= 10;
890   if (number > 0) rad /= 10;
891   while (rad)
892   {
893     serial_write((char)('0' + (number / rad)));
894     number -= (number / rad) * rad;
895     rad /= 10;
896   }  
897 }
898 
899 void serial_print_char(char ch)
900 {
901   serial_write(ch);  
902 }
903 
904 void serial_print(const uint8_t *str)
905 {
906   while (*str) serial_write(*(str++));
907 }
908 
909 void serial_println(const uint8_t *str)
910 {
911   serial_print(str);
912   serial_write(13);
913   serial_write(10);
914 }
915 
916 void serial_println_num(int32_t number)
917 {
918   serial_print_num(number);
919   serial_println();
920 }
921 
922 void serial_println_char(char ch)
923 {
924   serial_write(ch);
925   serial_println();
926 }
927 
928 void serial_println()
929 {
930   serial_write(13);
931   serial_write(10);
932 }
933 
934 // nasleduje citanie z utltazvukoveho senzora
935 
936 static volatile uint32_t pulse_start;
937 static volatile uint8_t new_distance;
938 
939 void init_ultrasonic()
940 {
941   pinMode(US_ECHO, INPUT);
942   pinMode(US_TRIG, OUTPUT);
943   
944   PCMSK0 |= 1; //PCINT0;
945   PCIFR &= ~1; //PCIF0;
946   PCICR |= 1; // PCIE0;  
947 }
948 
949 ISR(PCINT0_vect)
950 {
951   if (PINB & 1) pulse_start = micros();
952   else 
953   {
954     distance = (int16_t)((micros() - pulse_start) / 58);
955     new_distance = 1;
956   }
957 }
958 
959 void start_distance_measurement()
960 {
961   distance = 10000;
962   new_distance = 0;
963   digitalWrite(US_TRIG, HIGH);
964   delayMicroseconds(10);
965   digitalWrite(US_TRIG, LOW);
966 }
967 
968 void wait_for_distance_measurement_to_complete()
969 {
970   uint8_t counter = 0;
971   while ((counter < 20) && !new_distance) 
972   {
973     delay(1);
974     counter++;
975   }
976   if (counter == 20)
977   {
978     pinMode(US_ECHO, OUTPUT);
979     digitalWrite(US_ECHO, HIGH);
980     delayMicroseconds(10);
981     digitalWrite(US_ECHO, LOW);
982     pinMode(US_ECHO, INPUT); 
983     delayMicroseconds(5);
984     distance = 10000;
985   }
986 }
987 
988 int16_t measure_distance()
989 {
990   start_distance_measurement();
991   wait_for_distance_measurement_to_complete();
992   return distance;
993 }