Otto - riadiaci program v.2

From DT^2
Revision as of 10:30, 3 August 2018 by 158.195.189.186 (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.

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