Otto - riadiaci program v.2

From DT^2
Revision as of 08:59, 3 August 2018 by Admin (talk | contribs) (Created page with "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ô...")
(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.

klaves servo pin
a/q ľavá ruka 10
;/p pravá ruka 11
z/x ľavá noha 9
,/. pravá noha 6
d/c ľavá päta 5
k/m pravá päta 3
1/9 zníž/zvýš rýchlosť pohybu -
TXD BT 2
RXD BT 4
TRIG UZ 7
ECHO UZ 8
Sirena 12

Ďalej umožňuje

  • kalibrovať strednú polohu servomotorov (pomocou klávesu H)
    • pomocou + a - najdeme nulovu polohu serva
    • eneter ideme na dalsie servo
    • ukoncime E ulozenie do eprom
    • potvrdime Y
  • kalibrovať limity pre všetky stupne voľnosti (pomocou klávesu J), zobrazenie celej kalibrácie: kláves G
  • celú kalibráciu je možné zapísať do trvalej pamäte EEPROM pomocou klávesu E (používať opatrne!), po zapnutí sa automaticky načíta
  • pozdraví po klávese -, ruky dá hore na kláves R
  • načíta choreografiu po klávese @,
  • zobrazí načítanú choreografiu na kláves ? a
  • zatancuje načítanú choreografiu - kláves t.
  • Na klávesách 3-6 je možné nastaviť rôznu kombináciu (funkcie kombinacia1() - kombinacia4()).
  • Kláves U testuje ultrazvuk
  • Medzera resetne všetky servo motory do strednej polohy.

Pre komunikáciu na pinoch 0,1 pomocou bežného sériového portu treba pridať riadok #define pc Serial a zakomentovať riadky pc.begin(...); a SoftwareSerial pc(...)

  1 #include <Servo.h>
  2 #include <EEPROM.h>
  3 
  4 #define US_TRIG  7
  5 #define US_ECHO  8
  6 
  7 #define BT_RX   2 
  8 #define BT_TX   4
  9 
 10 #define SIRENA 12
 11 
 12 //maximalna dlzka choreografie
 13 #define CHOREO_LEN 200
 14 
 15 // cisla pinov, kde su zapojene servo motory
 16 #define PIN_SERVO_LAVA_RUKA   10 
 17 #define PIN_SERVO_PRAVA_RUKA  11
 18 #define PIN_SERVO_LAVA_NOHA   9
 19 #define PIN_SERVO_PRAVA_NOHA  6
 20 #define PIN_SERVO_LAVA_PATA   5
 21 #define PIN_SERVO_PRAVA_PATA  3
 22 
 23 #define S1 3
 24 #define S2 5
 25 #define S3 6
 26 #define S4 9
 27 #define S5 10
 28 #define S6 11
 29 #define SIRENA 12
 30 
 31 //maximalna dlzka choreografie
 32 #define CHOREO_LEN 200
 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[] = { 78, 69, 83, 80, 50, 67 };
 57 uint8_t prednastavena_kalibracia[] = { 90, 90, 90, 90, 90, 90 };
 58 
 59 uint8_t dolny_limit[] = {0, 0, 0, 0, 0, 0, 0, 0};
 60 uint8_t horny_limit[] = {180, 180, 180, 180, 180, 180};
 61 
 62 Servo s[6];
 63 
 64 uint16_t ch_time[CHOREO_LEN];
 65 uint8_t ch_servo[CHOREO_LEN];
 66 uint8_t ch_val[CHOREO_LEN];
 67 int ch_len;
 68 uint8_t kalib[6];
 69 int stav[6];
 70 int krok;
 71 
 72 void setup() {
 73   Serial.begin(9600);
 74   init_serial(9600);
 75   init_ultrasonic();
 76 
 77   randomSeed(analogRead(1));
 78   s[0].attach(PIN_SERVO_PRAVA_PATA);
 79   s[1].attach(PIN_SERVO_LAVA_PATA);
 80   s[2].attach(PIN_SERVO_PRAVA_NOHA);
 81   s[3].attach(PIN_SERVO_LAVA_NOHA);
 82   s[4].attach(PIN_SERVO_PRAVA_RUKA);
 83   s[5].attach(PIN_SERVO_LAVA_RUKA);
 84   precitaj_kalibraciu_z_EEPROM();
 85   for (int i = 0; i < 6; i++)
 86   {
 87     kalib[i] = prednastavena_kalibracia[i];
 88     stav[i] = kalib[i];
 89     s[i].write(stav[i]);
 90   }
 91   ch_len = 0;
 92   krok = 7;
 93   ahoj();
 94   ruky();
 95   delay(100);  
 96   serial_println("\r\n  Otto DTDT");
 97 }
 98 
 99 void loop() {
100   if (serial_available())
101   { 
102     char z = serial_read();
103     if (pohyb_znakom(z)) return;   
104     else if (pohyb_kombinacia(z)) return; 
105     else if (z == '@') nacitaj_choreografiu();
106     else if (z == '?') vypis_choreografiu();
107     else if (z == 't') zatancuj_choreografiu();
108     else if (z == '-') ahoj();
109     else if (z == ' ') reset();
110     else if (z == 'H') kalibruj();
111     else if (z == 'J') nastav_limity();
112     else if (z == 'G') vypis_kalibraciu();
113     else if (z == 'L') nacitaj_kalibraciu();
114     else if (z == 'E') zapis_kalibraciu_do_EEPROM();
115     else if (z == 'R') ruky();
116     else if (z == '9') zvys_krok();
117     else if (z == '1') zniz_krok();
118     else if (z == 'U') test_ultrazvuk();
119   }
120   int16_t d = measure_distance();
121   if (d < 10) menu_ultrasonic_request();
122 }
123 
124 void test_ultrazvuk()
125 {
126   while (serial_available() == 0)
127   {
128     serial_println_num(measure_distance());
129     delay(100);
130   }
131   serial_read();
132 }
133 
134 void menu_ultrasonic_request()
135 {
136   uint32_t tm = millis();
137   while ((millis() - tm < 3000) && (measure_distance() < 10)) delay(10);
138   if (measure_distance() < 15)
139     ultrasonic_menu();
140 }
141 
142 void ultrasonic_menu()
143 {
144   int selection = 0;
145   tone(SIRENA, 880, 200);
146   
147   do {  
148     int count = 0;
149     do { 
150       if (measure_distance() >= 20) count++;
151       else count = 0;
152       delay(10);
153     } while (count < 20);
154     
155     tone(SIRENA, 440, 200);
156     uint32_t tm = millis();
157     while ((measure_distance() > 15) && (millis() - tm < 1500)) delay(10);
158     if (millis() - tm >= 1500) 
159     {
160       tone(SIRENA, 2000, 50);
161       menu_command(selection);
162       return;
163     }       
164     selection++;
165     for (int i = 0; i < selection; i++)
166     {
167       tone(SIRENA, 1261, 50);
168       delay(250);
169     }
170   } while (1);
171 }
172 
173 void menu_command(int cmd)
174 {
175   serial_println_num(cmd);
176 }
177 
178 void precitaj_kalibraciu_z_EEPROM()
179 {
180   uint8_t value = EEPROM.read(1);
181   if (value != '~') return;
182   for (int i = 2; i < 8; i++)
183     prednastavena_kalibracia[i - 2] = EEPROM.read(i);
184   for (int i = 0; i < 6; i++)
185     dolny_limit[i] = EEPROM.read(i + 9);
186   for (int i = 0; i < 6; i++)
187     horny_limit[i] = EEPROM.read(i + 15);    
188 }
189 
190 void zapis_kalibraciu_do_EEPROM()
191 {
192   serial_print("Naozaj chces zapisat kalibraciu do EEPROM? [Y/n]: ");
193   while (!serial_available());
194   char odpoved = serial_read();
195   serial_println_char(odpoved);
196   if (odpoved == 'Y') 
197   {
198     EEPROM.write(1, '~');
199     for (int i = 2; i < 8; i++)
200       EEPROM.write(i, kalib[i - 2]);
201     for (int i = 0; i < 6; i++)
202       EEPROM.write(9 + i, dolny_limit[i]);
203     for (int i = 0; i < 6; i++)
204       EEPROM.write(15 + i, horny_limit[i]);
205     serial_println("ok");
206   }
207 }
208 
209 void pipni()
210 {
211   tone(SIRENA, 1568, 50);
212   delay(100);
213   tone(SIRENA, 1357, 50);
214 }
215 
216 void ruky()
217 {
218   int odloz_krok = krok;
219   delay(500);
220   krok = 90;
221   pohyb(SERVO_LAVA_RUKA);    
222   pohyb(SERVO_PRAVA_RUKA);
223   delay(1000);
224   krok = 180;
225   pohyb(-SERVO_LAVA_RUKA);    
226   pohyb(-SERVO_PRAVA_RUKA);
227   delay(1000);
228   krok = odloz_krok;
229   pipni();
230 }
231 
232 void ahoj()
233 {
234   tone(SIRENA, 1568, 50);
235   delay(70);
236   tone(SIRENA, 1175, 30);
237   delay(50);
238   tone(SIRENA, 880, 30);
239   delay(50);
240   tone(SIRENA, 1047, 50);
241   delay(70);
242   tone(SIRENA, 1245, 30);
243   delay(150);
244   tone(SIRENA, 1568, 50);
245   delay(100);
246   if (random(10) > 4) tone(SIRENA, 1357, 50);
247   else tone(SIRENA, 1047, 50);
248 }
249 
250 void nastav_koncatinu(int8_t servo, uint8_t poloha)
251 {
252   int8_t srv = (servo > 0)?servo:-servo;
253   srv--;
254   poloha += kalib[srv] - 90;
255   if (poloha > 180) poloha = 180;
256   if (poloha < 0) poloha = 0;
257   stav[srv] = poloha;
258   s[srv].write(stav[srv]);
259 }
260 
261 void pohyb(int8_t servo)
262 {
263   int8_t srv = (servo > 0)?servo:-servo;
264   srv--;
265   if (servo_invertovane[srv]) servo = -servo;
266   if (servo > 0)
267   {
268     if (stav[srv] <= horny_limit[srv] - krok) stav[srv] += krok;
269     else stav[srv] = horny_limit[srv];
270     s[srv].write(stav[srv]);
271   }
272   else if (servo < 0)
273   {
274     if (stav[srv] >= dolny_limit[srv] + krok) stav[srv] -= krok; 
275     else stav[srv] = dolny_limit[srv];
276     s[srv].write(stav[srv]);      
277   }
278 }
279 
280 uint8_t pohyb_znakom(char z)
281 {
282   for (int i = 0; i < 12; i++)
283   {
284     if (z == znaky_zmien[i])
285     {
286       int8_t servo = zmeny[i];
287       pohyb(servo);
288     }
289   }
290 }
291 
292 void kombinacia1()
293 {
294   pohyb(SERVO_LAVA_NOHA);
295   pohyb(-SERVO_PRAVA_PATA);
296 }
297 
298 void kombinacia2()
299 {
300   pohyb(SERVO_PRAVA_NOHA);
301   pohyb(-SERVO_LAVA_PATA);
302 }
303 
304 void kombinacia3()
305 {
306    pohyb(SERVO_LAVA_RUKA); 
307    pohyb(SERVO_PRAVA_RUKA); 
308 }
309 
310 void kombinacia4()
311 {
312    pohyb(-SERVO_LAVA_RUKA); 
313    pohyb(-SERVO_PRAVA_RUKA); 
314 }
315 
316 int pohyb_kombinacia(char z)
317 {
318   if (z == '3') kombinacia1();
319   else if (z == '4') kombinacia2();
320   else if (z == '5') kombinacia3();
321   else if (z == '6') kombinacia4();
322   else return 0;
323   return 1;
324 }
325 
326 int nacitajCislo()
327 {
328   int num = 0;
329   int z;
330   do {
331     z = serial_read();
332     if (z == '#') while (z != 13) z = serial_read();
333   } while ((z < '0') || (z > '9'));
334   while ((z >= '0') && (z <= '9'))
335   {
336     num *= 10;
337     num += (z - '0');
338     do { z = serial_read(); if (z == -1) delayMicroseconds(10); } while (z < 0);
339   }
340   return num;
341 }
342 
343 void nacitaj_choreografiu()
344 {
345   ch_len = 0;
346   int tm;
347   do { 
348     tm = nacitajCislo();
349     ch_time[ch_len] = tm;
350     ch_servo[ch_len] = nacitajCislo();
351     ch_val[ch_len] = nacitajCislo();
352     ch_len++;
353   if (ch_len == CHOREO_LEN) break;
354   } while (tm > 0);
355   pipni();  
356 }
357 
358 void vypis_choreografiu()
359 {
360   for (int i = 0; i < ch_len; i++)
361   {
362     serial_print_num(ch_time[i]);
363     serial_print(" ");
364     serial_print_num(ch_servo[i]);
365     serial_print(" ");
366     serial_println_num(ch_val[i]);      
367   }
368   pipni();
369 }
370 
371 void zatancuj_choreografiu()
372 {
373   for (int i = 0; i < ch_len; i++)
374   {
375     delay(ch_time[i]);
376     nastav_koncatinu(ch_servo[i], ch_val[i]);
377   }
378   if (ch_len > 0) stav[ch_servo[ch_len - 1]] = ch_val[ch_len - 1];
379   pipni();
380 }
381 
382 void reset()
383 {
384   for (int i = 0; i < 6; i++) 
385   {
386     stav[i] = kalib[i];
387     s[i].write(kalib[i]);
388   }
389   pipni();
390 }
391 
392 uint8_t nalad_hodnotu_serva(uint8_t servo, uint8_t hodnota)
393 {
394     serial_print(" (+/-/ENTER): ");
395     serial_println_num(hodnota);
396     s[servo].write(hodnota);
397     char z;
398     do {
399       z = serial_read();
400       if ((z == '+') && (hodnota < 180)) hodnota++;
401       else if ((z == '-') && (hodnota > 0)) hodnota--; 
402       if ((z == '+') || (z == '-'))
403       {
404         serial_print_num(hodnota); serial_print_char('\r'); 
405         s[servo].write(hodnota);
406       }
407     } while (z != 13);
408     return hodnota;
409 }
410 
411 void kalibruj()
412 {
413   for (int i = 0; i < 6; i++)
414   {
415     serial_print_num(i);
416     kalib[i] = nalad_hodnotu_serva(i, kalib[i]);
417     serial_print_num(i);
418     serial_print(": ");
419     serial_println_num(kalib[i]);
420   }
421   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
422   serial_println("ok");
423   pipni();
424 }
425 
426 void nastav_limity()
427 {
428   for (int i = 0; i < 6; i++)
429   {
430     serial_print_num(i);
431     serial_print("dolny");
432     dolny_limit[i] = nalad_hodnotu_serva(i, dolny_limit[i]);
433     serial_print_num(i);
434     serial_print(" dolny: ");
435     serial_println_num(dolny_limit[i]);
436     s[i].write(kalib[i]);
437 
438     serial_print_num(i);
439     serial_print("horny");
440     horny_limit[i] = nalad_hodnotu_serva(i, horny_limit[i]);
441     serial_print_num(i);
442     serial_print(" horny: ");
443     serial_println_num(horny_limit[i]);
444     s[i].write(kalib[i]);
445   }
446   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print("-"); serial_print_num(horny_limit[i]); serial_print(" "); }
447   serial_println("ok");
448   pipni();
449 }
450 
451 void vypis_kalibraciu()
452 {
453   serial_print("stredy: ");
454   for (int i = 0; i < 6; i++) { serial_print_num(kalib[i]); serial_print(" "); }
455   serial_println();
456   serial_print("dolny limit: ");
457   for (int i = 0; i < 6; i++) { serial_print_num(dolny_limit[i]); serial_print(" "); }
458   serial_println();
459   serial_print("horny limit: ");
460   for (int i = 0; i < 6; i++) { serial_print_num(horny_limit[i]); serial_print(" "); }
461   serial_println();
462 }
463 
464 void nacitaj_kalibraciu()
465 {
466   int tm;
467   for (int i = 0; i < 6; i++) 
468     kalib[i] = nacitajCislo();
469   vypis_kalibraciu();
470   serial_println("ok");
471   pipni();
472 }
473 
474 void zvys_krok()
475 {
476   if (krok < 180) krok++;
477   serial_print("krok: ");
478   serial_println_num(krok);
479 }
480 
481 void zniz_krok()
482 {
483   if (krok > 0) krok--;
484   serial_print("krok: ");
485   serial_println_num(krok);
486 }
487 
488 // nasleduje softverova implementacia serioveho portu
489 #define SERIAL_STATE_IDLE      0
490 #define SERIAL_STATE_RECEIVING 1
491 #define SERIAL_BUFFER_LENGTH   20
492 
493 static volatile uint8_t serial_state;
494 static uint8_t serial_buffer[SERIAL_BUFFER_LENGTH];
495 static volatile uint8_t serial_buf_wp, serial_buf_rp;
496 
497 static volatile uint8_t receiving_byte;
498 
499 static volatile uint32_t time_startbit_noticed;
500 static volatile uint8_t next_bit_order;
501 static volatile uint8_t waiting_stop_bit;
502 static uint16_t one_byte_duration;
503 static uint16_t one_bit_duration;
504 static uint16_t one_bit_write_duration;
505 static uint16_t half_of_one_bit_duration;
506 
507 void init_serial(uint32_t baud_rate)
508 {
509   pinMode(2, INPUT);
510   pinMode(4, OUTPUT);
511   
512   serial_state = SERIAL_STATE_IDLE;
513   
514   one_byte_duration = 9500000 / baud_rate;
515   one_bit_duration = 1000000 / baud_rate;
516   one_bit_write_duration = one_bit_duration - 5;
517   half_of_one_bit_duration = 500000 / baud_rate;
518   
519   PCMSK2 |= 4; //PCINT18;
520   PCIFR &= ~4; //PCIF2;
521   PCICR |= 4; // PCIE2;
522 }
523 
524 ISR(PCINT2_vect)
525 {
526   uint32_t tm = micros();
527   if (serial_state == SERIAL_STATE_IDLE)
528   {    
529     time_startbit_noticed = tm;
530     serial_state = SERIAL_STATE_RECEIVING;
531     receiving_byte = 0xFF;
532     next_bit_order = 0;
533   }
534   else if (tm - time_startbit_noticed > one_byte_duration)
535   {
536       serial_buffer[serial_buf_wp] = receiving_byte;
537       serial_buf_wp++;
538       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
539       time_startbit_noticed = tm;
540       receiving_byte = 0xFF;
541       next_bit_order = 0;
542   }
543   else if (PIND & 4) 
544   {
545      int8_t new_next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
546      while (next_bit_order < new_next_bit_order)
547      {  
548         receiving_byte &= ~(1 << next_bit_order);
549         next_bit_order++;
550      }
551      if (next_bit_order == 8)
552      { 
553         serial_buffer[serial_buf_wp] = receiving_byte;
554         serial_buf_wp++;
555         if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
556         serial_state = SERIAL_STATE_IDLE;
557      }        
558   } else 
559       next_bit_order = (tm - time_startbit_noticed - half_of_one_bit_duration) / one_bit_duration;
560 }
561 
562 uint8_t serial_available()
563 {
564   cli();
565   if (serial_buf_rp != serial_buf_wp) 
566   {
567     sei();
568     return 1;
569   }
570   if (serial_state == SERIAL_STATE_RECEIVING)
571   {
572     uint32_t tm = micros();
573     if (tm - time_startbit_noticed > one_byte_duration)
574     {      
575       serial_state = SERIAL_STATE_IDLE;
576       serial_buffer[serial_buf_wp] = receiving_byte;
577       serial_buf_wp++;
578       if (serial_buf_wp == SERIAL_BUFFER_LENGTH) serial_buf_wp = 0;
579       sei();
580       return 1;
581     }
582   }
583   sei();
584   return 0;
585 }
586 
587 int16_t serial_read()
588 {
589   cli();
590   if (serial_buf_rp != serial_buf_wp)
591   {
592     uint8_t ch = serial_buffer[serial_buf_rp];
593     serial_buf_rp++;
594     if (serial_buf_rp == SERIAL_BUFFER_LENGTH) serial_buf_rp = 0;
595     sei();
596     return ch;
597   }
598 
599   if (serial_state == SERIAL_STATE_RECEIVING)
600   {
601     uint32_t tm = micros();
602     if (tm - time_startbit_noticed > one_byte_duration)
603     {
604       uint8_t ch = receiving_byte;
605       serial_state = SERIAL_STATE_IDLE;  
606       sei();
607       return ch;
608     }
609   }
610   sei();
611   return -1;
612 }
613 
614 void serial_write(uint8_t ch)
615 {
616   PORTD &= ~16;
617   delayMicroseconds(one_bit_write_duration);
618   for (uint8_t i = 0; i < 8; i++)
619   {
620     if (ch & 1) PORTD |= 16;
621     else PORTD &= ~16;
622     ch >>= 1;
623     delayMicroseconds(one_bit_write_duration);
624   }
625   PORTD |= 16;
626   delayMicroseconds(one_bit_write_duration);
627   delayMicroseconds(one_bit_write_duration);
628   delayMicroseconds(one_bit_write_duration);
629 }
630 
631 uint16_t serial_readln(uint8_t *ln, uint16_t max_length)
632 {
633   uint16_t len;
634   uint16_t ch;
635   do {
636     ch = serial_read();
637     if (ch == 13) continue;
638   } while (ch == -1);
639 
640   do {
641     if ((ch != 13) && (ch != 10) && (ch != -1)) 
642     {
643       *(ln++) = ch;
644       max_length--;
645       len++;
646     }    
647     ch = serial_read();
648   } while ((ch != 13) && max_length);
649   *ln = 0;
650   return len;
651 }
652 
653 void serial_print_num(int number)
654 {
655   if (number < 0) 
656   {
657     serial_write('-');
658     number = -number;
659   }
660   int rad = 1;
661   while (number / rad) rad *= 10;
662   if (number > 0) rad /= 10;
663   while (rad)
664   {
665     serial_write((char)('0' + (number / rad)));
666     number -= (number / rad) * rad;
667     rad /= 10;
668   }  
669 }
670 
671 void serial_print_char(char ch)
672 {
673   serial_write(ch);  
674 }
675 
676 void serial_print(const uint8_t *str)
677 {
678   while (*str) serial_write(*(str++));
679 }
680 
681 void serial_println(const uint8_t *str)
682 {
683   serial_print(str);
684   serial_write(13);
685   serial_write(10);
686 }
687 
688 void serial_println_num(int number)
689 {
690   serial_print_num(number);
691   serial_println();
692 }
693 
694 void serial_println_char(char ch)
695 {
696   serial_write(ch);
697   serial_println();
698 }
699 
700 void serial_println()
701 {
702   serial_write(13);
703   serial_write(10);
704 }
705 
706 // nasleduje citanie z utltazvukoveho senzora
707 
708 static volatile uint32_t pulse_start;
709 static volatile int16_t distance;
710 static volatile uint8_t new_distance;
711 
712 void init_ultrasonic()
713 {
714   pinMode(US_ECHO, INPUT);
715   pinMode(US_TRIG, OUTPUT);
716   
717   PCMSK0 |= 1; //PCINT0;
718   PCIFR &= ~1; //PCIF0;
719   PCICR |= 1; // PCIE0;  
720 }
721 
722 ISR(PCINT0_vect)
723 {
724   if (PINB & 1) pulse_start = micros();
725   else 
726   {
727     distance = (int16_t)((micros() - pulse_start) / 58);
728     new_distance = 1;
729   }
730 }
731 
732 void start_distance_measurement()
733 {
734   distance = -1;
735   new_distance = 0;
736   digitalWrite(US_TRIG, HIGH);
737   delayMicroseconds(10);
738   digitalWrite(US_TRIG, LOW);
739 }
740 
741 void wait_for_distance_measurement_to_complete()
742 {
743   uint8_t counter = 0;
744   while ((counter < 20) && !new_distance) 
745   {
746     delay(1);
747     counter++;
748   }
749 }
750 
751 uint16_t measure_distance()
752 {
753   start_distance_measurement();
754   wait_for_distance_measurement_to_complete();
755   return distance;
756 }